#include "dji_flight_handler.h"
#include <dji_logger.h>
#include <math.h>
#include <thread>
#include <sys/types.h>
#include <dirent.h>
#include <string>
#include <vector>
#include "ac_pid.h"




using namespace std;


#define SBUF_SIZE 512

#define MOP_CHANNEL_TASK_STACK_SIZE                         2048
#define MOP_CHANNEL_INIT_TIMEMS                             (100)
#define MOP_CHANNEL_RETRY_TIMEMS                            (3 * 1000)

#define CIRCULAR_BUFFER_SIZE								(64 * 1024)
#define MOP_CHANNEL_BUFFER_SIZE								512

#define MOP_CHANNEL_NORMAL_TRANSFOR_CHANNEL_ID				666
#define MOP_CHANNEL_FILE_SERVICE_CHANNEL_ID					667


//relative with the liveView
static std::map<::E_DjiLiveViewCameraPosition, DJICameraStreamDecoder *> streamDecoder;

djiFlightHandler::djiFlightHandler() :
	m_osalHandler(NULL),
	m_subscriptionFlag(false),
	m_getLogDirFlag(false), 
	m_logFp(NULL),
	m_obtainJoyStickCtrlAuthorityFlag(NoMode),
	m_yawControlFunctionFlag(false),
	m_velocityControlFunctionFlag(false),
	m_positionControlFunctionFlag(false),
	m_startDjiLiveViewFlag(false),
	m_startFpvCameraStreamBoardModeFlag(NoMode),
	m_startMainCameraStreamBoardModeFlag(NoMode),
	m_startViceCameraStreamBoardModeFlag(NoMode),
	m_startTopCameraStreamBoardModeFlag(NoMode),
	m_quatLogCount(0),
	m_gimbalAnglesLogCount(0),
	m_velocityLogCount(0),
	m_avoidDataLogCount(0),
	m_mopChannelStartServiceFlag(false),
	m_mopChannelSendNormalTaskRunFlag(false),
	m_mopChannelRecvNormalTaskRunFlag(false),
	m_mopChannelConnectedFlag(false),
	m_flileMopChannelConnectedFlag(false),
	m_mopChannelReadySema(NULL),
	m_assistLandSystemStatus(assistLandStartError),
	m_assistAvoidSystemStatus(assistAvoidStartError),
	m_logListing(false),
	m_logSending(false),
	m_logSendingFlag(false),
	m_openReadFileFlag(false),
	m_logDataSize(0),
	m_logNumData(0),
	m_logDataOffset(0),
	m_logNextListEntry(0),
	m_logLastListEntry(0),
	m_logNumLogs(0),
	m_lastLogNum(0),
	m_readFd(NULL), 
	m_updateFlag(false),
	m_updateDataSize(0),
	m_updateOffsetCount(0),
	m_updateFp(NULL),
	m_landPointOk(false),
	m_canLandFlag(true),
	m_noHeartCount(0),
	m_cameraType((E_DjiCameraType)-1),
	m_cameraType2((E_DjiCameraType)-1),
	m_isTestMode(true),
	m_isFixBaseRTK(false)
{
	m_mopChannelSendBuffer.m_buffer = NULL;
	m_mopChannelRecvBuffer.m_buffer = NULL;
}


djiFlightHandler::~djiFlightHandler()
{
	m_mopChannelSendNormalTaskRunFlag = false;
	m_mopChannelRecvNormalTaskRunFlag = false;

	m_returnCode = DjiFcSubscription_DeInit();
	if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("Deinit fc subscription errcode:0x%08llX\n", m_returnCode);
	 }
	else
	{
		m_subscriptionFlag = false;
	}


	stopDjiLiveView();

	if(NULL != m_osalHandler)
	{
		free(m_osalHandler);
		m_osalHandler = NULL;
	}

	if(NULL != m_logFp)
	{
		fflush(m_logFp);
		fclose(m_logFp);
		m_logFp = NULL;
	}

	freeCircularBuf(&m_mopChannelSendBuffer);
	freeCircularBuf(&m_mopChannelRecvBuffer);
}


bool djiFlightHandler::startUpSystem()
{
    m_osalHandler = DjiPlatform_GetOsalHandler();

	if(NULL == m_osalHandler)
	{
		cout << "get osal handler failed" << endl;
		return false;
	}

	bool ret;

	// start hb log
	ret = getLogDir();
	if(!ret)
	{
		cout << "getLogDir failed" << endl; 
	}
	else
	{
		
		ret = createLogFile();
		if(!ret)
		{
			cout << "createLogFile failed" << endl;
		}
	}

	
	// start subscribe 
	ret = subscriptionInit();
	if(!ret)
	{
		cout << "subscription init failed" << endl;
		return false;
	}

	//start liveView
	ret = startDjiLiveView();
	if(!ret)
	{
		cout << "startDjiLiveView failed" << endl;
		//maybe other mode do not need liveview
		//return false;
	}

	//subscribe flight mode
	ret = subscriptionFlightMode();
	if(!ret)
	{
		cout << "subscriptionFlightMode failed" << endl;
		return false;
	}

	//subscribe flight status
	ret = subscriptionFlightStatus();
	if(!ret)
	{
		cout << "subsrcritionFlightMode failed" << endl;
		return false;
	}

	//subscribe veloctiy
	ret = subscriptionFlightVelocity();
	if(!ret)
	{
		cout << "subscriptionFlightVelocity failed" << endl;
		return false;
	}

	//subscribe position
	ret = subscriptionFlightPosition();
	if(!ret)
	{
		cout << "subscriptionFlightPostion failed" << endl;
		return false;
	}

	//subscribe relative height
	ret = subscriptionFlightRelativeHeight();
	if(!ret)
	{
		cout << "subscriptionRelativeHeight failed" << endl;
		return false;
	}

	//subscribe rtk connect status
	ret = subscriptionRtkConnectStatus();
	if(!ret)
	{
		cout << "subscriptionRtkConnectStatus failed" << endl;
		return false;
	}

	//subscribe rtk position information
	ret = subscriptionRtkPositionInfo();
	if(!ret)
	{
		cout << "subscriptionRtkPostionInfo failed" << endl;
		return false;
	}

	//subscribe rtk yaw information 
	ret = subscriptionRtkYawInfo();
	if(!ret)
	{
		cout << "subscriptionRtkYawInfo failed" << endl;
		return false;
	}

	//subscribe flight quaternion
	ret = subscriptionFlightQuaternion();
	if(!ret)
	{
		cout << "subscriptionFlightQuaternion failed" << endl;
		return false;
	}

	//subscribe rtkPosition
	ret = subscriptionRtkPosition();
	if(!ret)
	{
		cout << "subscriptionRtkPosition failed" << endl;
		return false;
	}

	//subscribe gps details
	ret = subscriptionGpsDetail();
	if(!ret)
	{
		cout << "subscriptionGpsDetail failed" << endl;
		return false;
	}

	//subsribe height Fusion
	ret = subscriptionHeightFusion();
	if(!ret)
	{
		cout << "subscriptionHeightFusion failed" << endl;
		return false;
	}

	//subsribe altitude fused
	ret = subscriptionAltitudeFused();
	if(!ret)
	{
		cout << "subscriptionAltitudeFused failed" << endl;
		return false;
	}

	//subsribe altitude barometer
	ret = subscriptionAltitudeBarometer();
	if(!ret)
	{
		cout << "subscriptionAltitudeBarometer failed" << endl;
		return false;
	}

	//subsribe altitude of homePoint
	ret = subscriptionAltitudeHomePoint();
	if(!ret)
	{
		cout << "subscriptionAltitudeHomePoint failed" << endl;
		return false;
	}

	//subsribe gimbal angles
	ret = subscriptionGimbalAngles();
	if(!ret)
	{
		cout << "subscriptionGimbalAngles failed" << endl;
		//maybe others do not need gimbal data.
		//return false;
	}

	//subsribe battery information
	ret = subscriptionWholeBatteryInfo();
	if(!ret)
	{
		cout << "subscriptionWholeBatteryInfo failed" << endl;
		//return false;
	}

	//subsribe avoid data
	ret = subscriptionAvoidData();
	if(!ret)
	{
		cout << "subscriptionAvoidData failed" << endl;
		//return false;
	}

#if 0  //failed, I don't know why.
	ret = subscriptionGimbalStatus();
	if(!ret);
	{
		cout << "subscriptionGimbalStatus failed" <<endl;
		//maybe others do not need gimbal data.
		//return false;
	}
#endif

	ret = subscriptionGimbalControlMOde();
	if(!ret)
	{
		cout << "subsriptionGimbalControlMOde failed" <<endl;
		//maybe others do not need gimbal data.
		//return false;
	}

	//start the communication task
	ret = mopChannelStartService();
	if(!ret)
	{
		//cout << "mopChannelStartService failed" <<endl;
		//return false;
	}

	return true;
}

void djiFlightHandler::sleepMs(uint32_t timeMs)
{
	m_osalHandler->TaskSleepMs(timeMs);
}

bool djiFlightHandler::semaphoreCreate(uint32_t initValue, T_DjiSemaHandle *semaphore)
{
	m_returnCode = m_osalHandler->SemaphoreCreate(initValue, semaphore);
	if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("semaphoreCreate errcode:0x%08llX\n", m_returnCode);
		return false;
	}

	return true;
}


bool djiFlightHandler::semaphorePost()
{
	m_returnCode = m_osalHandler->SemaphorePost(m_mopChannelReadySema);
	if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("semaphorePost, error code:0x%08llX \n", m_returnCode);
		return false;
	}

	return true;
}


bool djiFlightHandler::semaphoreWait()
{
	m_returnCode = m_osalHandler->SemaphoreWait(m_mopChannelReadySema);
	if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("semaphoreWait, error code:0x%08llX \n", m_returnCode);
		return false;
	}

	return true;
}



void djiFlightHandler::getCurrentTimeMs(uint32_t &timeMs)
{
	m_osalHandler->GetTimeMs(&timeMs);
}


bool djiFlightHandler::subscriptionInit()
{
	m_returnCode = DjiFcSubscription_Init();
    if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("subscriptionInit errcode:0x%08llX\n", m_returnCode);
        return false;
    }

	m_subscriptionFlag = true;

	return true;
}

static T_DjiReturnCode SubscriptionFlightModeCallback(const uint8_t *data, uint16_t dataSize,
                                                                       const T_DjiDataTimestamp *timestamp)
{
	T_DjiFcSubscriptionDisplaymode *displayMode = (T_DjiFcSubscriptionDisplaymode *) data;

	if(sizeof(T_DjiFcSubscriptionDisplaymode) != dataSize)
	{
		flightHandler.writeLogFile("SubscriptionFlightModeCallback sizof() != dataSize\n");
		return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
	}
	
	flightHandler.setFlightMode(displayMode, timestamp);

	return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}


bool djiFlightHandler::subscriptionFlightMode()
{
	if(!m_subscriptionFlag)
	{
		return false;
	}

	m_returnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_STATUS_DISPLAYMODE,
												DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ,
												SubscriptionFlightModeCallback);
    if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
    {
    	writeLogFile("subscriptionFlightMode errcode:0x%08llX\n", m_returnCode);
        return false;
    }

	return true;
}


static T_DjiReturnCode SubscriptionFlightStatusCallback(const uint8_t *data, uint16_t dataSize,
                                                                       const T_DjiDataTimestamp *timestamp)
{
	T_DjiFcSubscriptionFlightStatus *status = (T_DjiFcSubscriptionFlightStatus *) data;

	if(sizeof(T_DjiFcSubscriptionFlightStatus) != dataSize)
	{
		flightHandler.writeLogFile("SubscriptionFlightStatusCallback sizof() != dataSize\n");
		return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
	}
	
	flightHandler.setFlightStatus(status, timestamp);

	return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

bool djiFlightHandler::subscriptionFlightStatus()
{
	if(!m_subscriptionFlag)
	{
		return false;
	}

	m_returnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_STATUS_FLIGHT,
												DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ,
												SubscriptionFlightStatusCallback);
    if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
    {
    	writeLogFile("subscriptionFlightStatus errcode:0x%08llX\n", m_returnCode);
        return false;
    }

	return true;
}



static T_DjiReturnCode SubscriptionFlightVelocityCallback(const uint8_t *data, uint16_t dataSize,
                                                                       const T_DjiDataTimestamp *timestamp)
{
	T_DjiFcSubscriptionVelocity *velocity = (T_DjiFcSubscriptionVelocity *) data;

	if(sizeof(T_DjiFcSubscriptionVelocity) != dataSize)
	{
		flightHandler.writeLogFile("SubscriptionFlightVelocityCallback sizof() != dataSize\n");
		return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
	}

	flightHandler.setFlightVelocity(velocity, timestamp);

	return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}


bool djiFlightHandler::subscriptionFlightVelocity()
{
	if(!m_subscriptionFlag)
	{
		return false;
	}

	m_returnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_VELOCITY,
												DJI_DATA_SUBSCRIPTION_TOPIC_200_HZ,
												SubscriptionFlightVelocityCallback);

	if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
    {
    	writeLogFile("subscriptionFlightVelocity errcode:0x%08llX\n", m_returnCode);
        return false;
    }

	return true;
}


static T_DjiReturnCode SubscriptionFlightPositionCallback(const uint8_t *data, uint16_t dataSize,
                                                                       const T_DjiDataTimestamp *timestamp)
{
	T_DjiFcSubscriptionPositionFused *position = (T_DjiFcSubscriptionPositionFused *) data;

	if(sizeof(T_DjiFcSubscriptionPositionFused) != dataSize)
	{
		flightHandler.writeLogFile("SubscriptionFlightPositionCallback sizof() != dataSize\n");
		return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
	}

	flightHandler.setFlightPosition(position, timestamp);

	return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}


bool djiFlightHandler::subscriptionFlightPosition()
{
	if(!m_subscriptionFlag)
	{
		return false;
	}

	m_returnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_POSITION_FUSED,
												DJI_DATA_SUBSCRIPTION_TOPIC_5_HZ,
												SubscriptionFlightPositionCallback);

	if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
    {
    	writeLogFile("subscriptionFlightPosition errcode:0x%08llX\n", m_returnCode);
        return false;
    }

	return true;
}

static T_DjiReturnCode SubscriptionFlightRelativeHeightCallback(const uint8_t *data, uint16_t dataSize,
                                                                       const T_DjiDataTimestamp *timestamp)
{
	T_DjiFcSubscriptionHeightRelative *relativeHeight = (T_DjiFcSubscriptionHeightRelative *) data;

	if(sizeof(T_DjiFcSubscriptionHeightRelative) != dataSize)
	{
		flightHandler.writeLogFile("SubscriptionFlightRelativeHeightCallback sizof() != dataSize\n");
		return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
	}

	flightHandler.setFlightRelativeHeight(relativeHeight, timestamp);

	return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}



bool djiFlightHandler::subscriptionFlightRelativeHeight()
{
	if(!m_subscriptionFlag)
	{
		return false;
	}

	m_returnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_HEIGHT_RELATIVE,
												DJI_DATA_SUBSCRIPTION_TOPIC_5_HZ,
												SubscriptionFlightRelativeHeightCallback);

	if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
    {
    	writeLogFile("subscriptionFlightRelativeHeight errcode:0x%08llX\n", m_returnCode);
        return false;
    }

	return true;
}

static T_DjiReturnCode SubscriptionRtkConnectStatusCallback(const uint8_t *data, uint16_t dataSize,
                                                                       const T_DjiDataTimestamp *timestamp)
{
	T_DjiFcSubscriptionRTKConnectStatus *rtkConnectStatus = (T_DjiFcSubscriptionRTKConnectStatus *) data;

	if(sizeof(T_DjiFcSubscriptionRTKConnectStatus) != dataSize)
	{
		flightHandler.writeLogFile("SubscriptionRtkConnectStatusCallback sizof() != dataSize\n");
		return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
	}

	flightHandler.setRtkConnectStatus(rtkConnectStatus, timestamp);

	return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}


bool djiFlightHandler::subscriptionRtkConnectStatus()
{
	if(!m_subscriptionFlag)
	{
		return false;
	}

	m_returnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_RTK_CONNECT_STATUS,
												DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ,
												SubscriptionRtkConnectStatusCallback);

	if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
    {
    	writeLogFile("subscriptionRtkConnectStatus errcode:0x%08llX\n", m_returnCode);
        return false;
    }

	return true;
}


static T_DjiReturnCode SubscriptionRtkPositionInfoCallback(const uint8_t *data, uint16_t dataSize,
                                                                       const T_DjiDataTimestamp *timestamp)
{
	T_DjiFcSubscriptionRtkPositionInfo *rtkPositionInfo = (T_DjiFcSubscriptionRtkPositionInfo *) data;

	if(sizeof(T_DjiFcSubscriptionRtkPositionInfo) != dataSize)
	{
		flightHandler.writeLogFile("SubscriptionRtkPositionInfoCallback sizof() != dataSize\n");
		return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
	}

	flightHandler.setRtkPositionInfo(rtkPositionInfo, timestamp);

	return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}



bool djiFlightHandler::subscriptionRtkPositionInfo()
{
	if(!m_subscriptionFlag)
	{
		return false;
	}

	m_returnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_RTK_POSITION_INFO,
												DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ,
												SubscriptionRtkPositionInfoCallback);

	if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
    {
    	writeLogFile("subscriptionRtkPositionInfo errcode:0x%08llX\n", m_returnCode);
        return false;
    }

	return true;
}

static T_DjiReturnCode SubscriptionRtkYawInfoCallback(const uint8_t *data, uint16_t dataSize,
                                                                       const T_DjiDataTimestamp *timestamp)
{
	T_DjiFcSubscriptionRtkYawInfo *rtkYawInfo = (T_DjiFcSubscriptionRtkYawInfo *) data;

	if(sizeof(T_DjiFcSubscriptionRtkYawInfo) != dataSize)
	{
		flightHandler.writeLogFile("SubscriptionRtkYawInfoCallback sizof() != dataSize\n");
		return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
	}

	flightHandler.setRtkYawInfo(rtkYawInfo, timestamp);

	return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}


bool djiFlightHandler::subscriptionRtkYawInfo()
{
	if(!m_subscriptionFlag)
	{
		return false;
	}

	m_returnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_RTK_YAW_INFO,
												DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ,
												SubscriptionRtkYawInfoCallback);

	if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
    {
    	writeLogFile("subscriptionRtkYawInfo errcode:0x%08llX\n", m_returnCode);
        return false;
    }

	return true;
}


static T_DjiReturnCode SubscriptionFlightQuaternionCallback(const uint8_t *data, uint16_t dataSize,
                                                                       const T_DjiDataTimestamp *timestamp)
{
    T_DjiFcSubscriptionQuaternion *quaternion = (T_DjiFcSubscriptionQuaternion *) data;

    if(sizeof(T_DjiFcSubscriptionQuaternion) != dataSize)
    {
    	flightHandler.writeLogFile("SubscriptionFlightQuaternionCallback sizof() != dataSize\n");
		return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
	}

	flightHandler.setFlightQuat(quaternion, timestamp);

    return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}


bool djiFlightHandler::subscriptionFlightQuaternion()
{
	if(!m_subscriptionFlag)
	{
		return false;
	}

	m_returnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_QUATERNION,
												DJI_DATA_SUBSCRIPTION_TOPIC_200_HZ,
												SubscriptionFlightQuaternionCallback);

	if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
    {
    	writeLogFile("subscriptionFlightQuaternion errcode:0x%08llX\n", m_returnCode);
        return false;
    }

	return true;
}


static T_DjiReturnCode SubscriptionRtkPositionCallback(const uint8_t *data, uint16_t dataSize,
                                                                       const T_DjiDataTimestamp *timestamp)
{
    T_DjiFcSubscriptionRtkPosition *rtkPosition = (T_DjiFcSubscriptionRtkPosition *) data;

    if(sizeof(T_DjiFcSubscriptionRtkPosition) != dataSize)
    {
    	flightHandler.writeLogFile("SubscriptionRtkPositionCallback sizof() != dataSize\n");
		return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
	}

	flightHandler.setRtkPosition(rtkPosition, timestamp);

    return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}


bool djiFlightHandler::subscriptionRtkPosition()
{
	if(!m_subscriptionFlag)
	{
		return false;
	}

	m_returnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_RTK_POSITION,
												DJI_DATA_SUBSCRIPTION_TOPIC_5_HZ,
												SubscriptionRtkPositionCallback);

	if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
    {
    	writeLogFile("subscriptionRtkPosition errcode:0x%08llX\n", m_returnCode);
        return false;
    }

	return true;
}


static T_DjiReturnCode SubscriptionGpsDetailCallback(const uint8_t *data, uint16_t dataSize,
                                                                       const T_DjiDataTimestamp *timestamp)
{
    T_DjiFcSubscriptionGpsDetails *gpsDetail = (T_DjiFcSubscriptionGpsDetails *) data;

    if(sizeof(T_DjiFcSubscriptionGpsDetails) != dataSize)
    {
    	flightHandler.writeLogFile("SubscriptionGpsDetailCallback sizof() != dataSize\n");
		return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
	}

	flightHandler.setGpsDetails(gpsDetail, timestamp);

    return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}


bool djiFlightHandler::subscriptionGpsDetail()
{
	if(!m_subscriptionFlag)
	{
		return false;
	}

	m_returnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_DETAILS,
												DJI_DATA_SUBSCRIPTION_TOPIC_5_HZ,
												SubscriptionGpsDetailCallback);

	if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
    {
    	writeLogFile("subscriptionGpsDetail errcode:0x%08llX\n", m_returnCode);
        return false;
    }

	return true;
}

static T_DjiReturnCode SubscriptionHeightFusionCallback(const uint8_t *data, uint16_t dataSize,
                                                                       const T_DjiDataTimestamp *timestamp)
{
    T_DjiFcSubscriptionHeightFusion *heightFusion = (T_DjiFcSubscriptionHeightFusion *) data;

    if(sizeof(T_DjiFcSubscriptionHeightFusion) != dataSize)
    {
    	flightHandler.writeLogFile("SubscriptionHeightFusionCallback sizof() != dataSize\n");
		return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
	}

	flightHandler.setHeightFusion(heightFusion, timestamp);

    return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}


bool djiFlightHandler::subscriptionHeightFusion()
{
	if(!m_subscriptionFlag)
	{
		return false;
	}

	m_returnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_HEIGHT_FUSION,
												DJI_DATA_SUBSCRIPTION_TOPIC_5_HZ,
												SubscriptionHeightFusionCallback);

	if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
    {
    	writeLogFile("subscriptionHeightFusion errcode:0x%08llX\n", m_returnCode);
        return false;
    }

	return true;
}


static T_DjiReturnCode SubscriptionGimbalAnglesCallback(const uint8_t *data, uint16_t dataSize,
                                                                       const T_DjiDataTimestamp *timestamp)
{
    T_DjiFcSubscriptionGimbalAngles *gimbalAnglesPry = (T_DjiFcSubscriptionGimbalAngles *) data;

    if(sizeof(T_DjiFcSubscriptionGimbalAngles) != dataSize)
    {
    	flightHandler.writeLogFile("SubscriptionGimbalAnglesCallback sizof() != dataSize\n");
		return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
	}

	flightHandler.setGimbalAngles(gimbalAnglesPry, timestamp);

    return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}



bool djiFlightHandler::subscriptionGimbalAngles()
{
	if(!m_subscriptionFlag)
	{
		return false;
	}

	m_returnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_GIMBAL_ANGLES,
												DJI_DATA_SUBSCRIPTION_TOPIC_50_HZ,
												SubscriptionGimbalAnglesCallback);

	if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
    {
    	writeLogFile("subscriptionGimbalAngles errcode:0x%08llX\n", m_returnCode);
        return false;
    }

	return true;
}


static T_DjiReturnCode SubscriptionGimbalStatusCallback(const uint8_t *data, uint16_t dataSize,
                                                                       const T_DjiDataTimestamp *timestamp)
{
    T_DjiFcSubscriptionGimbalStatus *gimbalStatus = (T_DjiFcSubscriptionGimbalStatus *) data;

    if(sizeof(T_DjiFcSubscriptionGimbalStatus) != dataSize)
    {
    	flightHandler.writeLogFile("SubscriptionGimbalStatusCallback sizof() != dataSize\n");
		return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
	}

	flightHandler.setGimbalStatus(gimbalStatus, timestamp);

    return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}


bool djiFlightHandler::subscriptionGimbalStatus()
{
	if(!m_subscriptionFlag)
	{
		return false;
	}

	m_returnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_GIMBAL_STATUS,
												DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ,
												SubscriptionGimbalStatusCallback);

	if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
    {
    	writeLogFile("subscriptionGimbalStatus errcode:0x%08llX\n", m_returnCode);
        return false;
    }

	return true;
}


static T_DjiReturnCode SubscriptionGimbalControlModeCallback(const uint8_t *data, uint16_t dataSize,
                                                                       const T_DjiDataTimestamp *timestamp)
{
    T_DjiFcSubscriptionGimbalControlMode *gimbalControlMode = (T_DjiFcSubscriptionGimbalControlMode *) data;

    if(sizeof(T_DjiFcSubscriptionGimbalControlMode) != dataSize)
    {
    	flightHandler.writeLogFile("SubscriptionGimbalControlModeCallback sizof() != dataSize\n");
		return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
	}

	flightHandler.setGimbalControlMode(gimbalControlMode, timestamp);

    return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}


bool djiFlightHandler::subscriptionGimbalControlMOde()
{
	if(!m_subscriptionFlag)
	{
		return false;
	}

	m_returnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_GIMBAL_CONTROL_MODE,
												DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ,
												SubscriptionGimbalControlModeCallback);

	if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
    {
    	writeLogFile("subscriptionGimbalStatus errcode:0x%08llX\n", m_returnCode);
        return false;
    }

	return true;
}



static T_DjiReturnCode SubscriptionAltitudeFusedCallback(const uint8_t *data, uint16_t dataSize,
                                                                       const T_DjiDataTimestamp *timestamp)
{
    T_DjiFcSubscriptionAltitudeFused *altitudeFused = (T_DjiFcSubscriptionAltitudeFused *) data;

    if(sizeof(T_DjiFcSubscriptionAltitudeFused) != dataSize)
    {
    	flightHandler.writeLogFile("SubscriptionAltitudeFusedCallback sizof() != dataSize\n");
		return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
	}

	flightHandler.setAltitudeFused(altitudeFused, timestamp);

    return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}

bool djiFlightHandler::subscriptionAltitudeFused()
{
	if(!m_subscriptionFlag)
	{
		return false;
	}

	m_returnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_FUSED,
												DJI_DATA_SUBSCRIPTION_TOPIC_5_HZ,
												SubscriptionAltitudeFusedCallback);


	if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
    {
    	writeLogFile("subscriptionAltitudeFused errcode:0x%08llX\n", m_returnCode);
        return false;
    }

	return true;

}


static T_DjiReturnCode SubscriptionAltitudeBarometerCallback(const uint8_t *data, uint16_t dataSize,
                                                                       const T_DjiDataTimestamp *timestamp)
{
    T_DjiFcSubscriptionAltitudeBarometer *altitudeBarometer = (T_DjiFcSubscriptionAltitudeBarometer *) data;

    if(sizeof(T_DjiFcSubscriptionAltitudeBarometer) != dataSize)
    {
    	flightHandler.writeLogFile("SubscriptionAltitudeBarometerCallback sizof() != dataSize\n");
		return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
	}

	flightHandler.setAltitudeBarometer(altitudeBarometer, timestamp);

    return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}


bool djiFlightHandler::subscriptionAltitudeBarometer()
{
	if(!m_subscriptionFlag)
	{
		return false;
	}

	m_returnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_BAROMETER,
												DJI_DATA_SUBSCRIPTION_TOPIC_5_HZ,
												SubscriptionAltitudeBarometerCallback);


	if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
    {
    	writeLogFile("subscriptionAltitudeBarometer errcode:0x%08llX\n", m_returnCode);
        return false;
    }

	return true;

}


static T_DjiReturnCode SubscriptionAltitudeHomePointCallback(const uint8_t *data, uint16_t dataSize,
                                                                       const T_DjiDataTimestamp *timestamp)
{
    T_DjiFcSubscriptionAltitudeOfHomePoint *altitudeBarometer = (T_DjiFcSubscriptionAltitudeOfHomePoint *) data;

    if(sizeof(T_DjiFcSubscriptionAltitudeOfHomePoint) != dataSize)
    {
    	flightHandler.writeLogFile("SubscriptionAltitudeHomePointCallback sizof() != dataSize\n");
		return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
	}

	flightHandler.setAltitudeHomePoint(altitudeBarometer, timestamp);

    return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}


bool djiFlightHandler::subscriptionAltitudeHomePoint()
{
	if(!m_subscriptionFlag)
	{
		return false;
	}

	m_returnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_ALTITUDE_OF_HOMEPOINT,
												DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ,
												SubscriptionAltitudeHomePointCallback);

	if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
    {
    	writeLogFile("subscriptionAltitudeHomePoint errcode:0x%08llX\n", m_returnCode);
        return false;
    }

	return true;
}


static T_DjiReturnCode SubscriptionWholeBatteryInfoCallback(const uint8_t *data, uint16_t dataSize,
                                                                       const T_DjiDataTimestamp *timestamp)
{
	T_DjiFcSubscriptionWholeBatteryInfo *batteryInfo = (T_DjiFcSubscriptionWholeBatteryInfo *) data;

	if(sizeof(T_DjiFcSubscriptionWholeBatteryInfo) != dataSize)
	{
		flightHandler.writeLogFile("SubscriptionWholeBatteryInfoCallback sizof() != dataSize\n");
		return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
	}

	flightHandler.setBatteryInfo(batteryInfo, timestamp);

	return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}



bool djiFlightHandler::subscriptionWholeBatteryInfo()
{
	if(!m_subscriptionFlag)
	{
		return false;
	}

	m_returnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_BATTERY_INFO,
												DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ,
												SubscriptionWholeBatteryInfoCallback);

	if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("subscriptionWholeBatteryInfo errcode:0x%08llX\n", m_returnCode);
		return false;
	}

	return true;

}


static T_DjiReturnCode SubscriptionAvoidDataCallback(const uint8_t *data, uint16_t dataSize,
                                                                       const T_DjiDataTimestamp *timestamp)
{
	T_DjiFcSubscriptionAvoidData *avoidData = (T_DjiFcSubscriptionAvoidData *) data;

	if(sizeof(T_DjiFcSubscriptionAvoidData) != dataSize)
	{
		flightHandler.writeLogFile("SubscriptionAvoidDataCallback sizof() != dataSize\n");
		return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
	}

	flightHandler.setAvoidData(avoidData, timestamp);

	return DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}


bool djiFlightHandler::subscriptionAvoidData()
{
	if(!m_subscriptionFlag)
	{
		return false;
	}

	m_returnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_AVOID_DATA,
												DJI_DATA_SUBSCRIPTION_TOPIC_50_HZ,
												SubscriptionAvoidDataCallback);

	if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("subscriptionAvoidData errcode:0x%08llX\n", m_returnCode);
		return false;
	}

	return true;
}




bool djiFlightHandler::startDjiLiveView()
{
    m_returnCode = DjiLiveview_Init();
    if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("startDjiLiveView errcode:0x%08llX\n", m_returnCode);
       	return false;
    }

    streamDecoder = {
        {DJI_LIVEVIEW_CAMERA_POSITION_FPV,  (new DJICameraStreamDecoder())},
        {DJI_LIVEVIEW_CAMERA_POSITION_NO_1, (new DJICameraStreamDecoder())},
        {DJI_LIVEVIEW_CAMERA_POSITION_NO_2, (new DJICameraStreamDecoder())},
        {DJI_LIVEVIEW_CAMERA_POSITION_NO_3, (new DJICameraStreamDecoder())},
    };

	m_startDjiLiveViewFlag = true;
	return true;
}

void djiFlightHandler::stopDjiLiveView()
{

	m_startDjiLiveViewFlag = false;
    m_returnCode = DjiLiveview_Deinit();
    if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
        writeLogFile("stopDjiLiveView errcode:0x%08llX\n", m_returnCode);
		return ;
    }

    for (auto pair : streamDecoder)
	{
        if (pair.second)
		{
            delete pair.second;
        }
    }
}


int removeSEINal(unsigned char *pBuf, const uint8_t * data, uint32_t len, bool &isKeyFrame)
{
	unsigned char naluType;
	uint32_t dataLen = 0;
	bool drop = false;
	int nalNum = 0;

	for(int i=0; i<len; i++)
	{
		if(data[i] == 0 && data[i+1] == 0 && data[i+2] == 0 && data[i+3] == 1)
		{
			naluType = data[i+4] & 0x1F;

			switch(naluType)
			{
				case 0x06:
					drop = true;
					break;
				case 0x07:
				case 0x08:
					nalNum++;
					drop = false;
					break;
				default:
					drop = false;
					break;
			}
		}

		if(drop)
		{
			continue;
		}
		else
		{
			if(nalNum > 0)
			{
				isKeyFrame = true;
			}
			else
			{
				isKeyFrame = false;
			}

			*pBuf++ = data[i];
			dataLen++;
		}
	}

	return dataLen;
}


FILE *testH264 = NULL;

/* Private functions definition-----------------------------------------------*/
static void liveViewConvertH264ToRgbCallback(E_DjiLiveViewCameraPosition position, const uint8_t *buf, uint32_t bufLen)
{
#if 0 //for test
	if(testH264 == NULL)
	{
		testH264 =  fopen("/testImage/test.h264", "ab+");
	}

	if(testH264 != NULL)
	{
		fwrite(buf, 1, bufLen, testH264);
		fflush(testH264);
	}
#endif

    auto deocder = streamDecoder.find(position);
    if ((deocder != streamDecoder.end()) && deocder->second) {
        deocder->second->decodeBuffer(buf, bufLen);
    }
}


bool djiFlightHandler::startFpvCameraStream(enum OnBoardModeType mode, CameraImageCallback callback, void *userData, E_DjiLiveViewCameraSource source)
{
	if(!m_startDjiLiveViewFlag)
	{
		writeLogFile("wanter:%d execute startFpvCameraStream, but startDjiLiveViewFlag is false\n", mode);
		return false;
	}

	if(NoMode != m_startFpvCameraStreamBoardModeFlag)
	{
		writeLogFile("wanter:%d startFpvCameraStream, but holder:%d\n", mode, m_startFpvCameraStreamBoardModeFlag);
		return false;
	}

	auto deocder = streamDecoder.find(DJI_LIVEVIEW_CAMERA_POSITION_FPV);

	if ((deocder != streamDecoder.end()) && deocder->second)
	{
        deocder->second->init();
        deocder->second->registerCallback(callback, userData);

        m_returnCode = DjiLiveview_StartH264Stream(DJI_LIVEVIEW_CAMERA_POSITION_FPV, source,
                                           liveViewConvertH264ToRgbCallback);

		if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
		{
        	writeLogFile("wanter:%d StartFpvCameraStream errcode:0x%08llX\n",mode, m_returnCode);
			return false;
    	}

		m_startFpvCameraStreamBoardModeFlag = mode;
		writeLogFile("wanter: %d startFpvCameraStream success\n", mode);
		return true;
    }

	writeLogFile("wanter: %d startFpvCameraStream find DJI_LIVEVIEW_CAMERA_POSITION_FPV error\n", mode);
	return false;
}


bool djiFlightHandler::stopFpvCameraStream(enum OnBoardModeType mode, E_DjiLiveViewCameraSource source)
{
	if(!m_startDjiLiveViewFlag)
	{
		writeLogFile("wanter:%d execute stopFpvCameraStream, but startDjiLiveViewFlag is false\n", mode);
		return false;
	}

	if(mode != m_startFpvCameraStreamBoardModeFlag)
	{
		writeLogFile("wanter:%d stopFpvCameraStream, but holder:%d\n", mode, m_startFpvCameraStreamBoardModeFlag);
		return false;
	}

	m_returnCode = DjiLiveview_StopH264Stream(DJI_LIVEVIEW_CAMERA_POSITION_FPV, source);
    if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		//sorry, I don't know why
		m_startFpvCameraStreamBoardModeFlag = NoMode;
		writeLogFile("holder %d stopFpvCameraStream errcode:0x%08llX\n", mode);
        return false;
    }

    auto deocder = streamDecoder.find(DJI_LIVEVIEW_CAMERA_POSITION_FPV);
    if ((deocder != streamDecoder.end()) && deocder->second)
	{
        deocder->second->cleanup();
    }

	m_startFpvCameraStreamBoardModeFlag = NoMode;
	writeLogFile("holder %d stopFpvCameraStream success\n", mode);
	return true;
}

bool djiFlightHandler::startMainCameraStream(enum OnBoardModeType mode, CameraImageCallback callback, void *userData, E_DjiLiveViewCameraSource source)
{
	if(!m_startDjiLiveViewFlag)
	{
		writeLogFile("wanter:%d execute startMainCameraStream, but startDjiLiveViewFlag is false\n", mode);
		return false;
	}

	if(NoMode != m_startMainCameraStreamBoardModeFlag)
	{
		writeLogFile("wanter:%d startMainCameraStream, but holder:%d\n", mode, m_startMainCameraStreamBoardModeFlag);
		return false;
	}

	auto deocder = streamDecoder.find(DJI_LIVEVIEW_CAMERA_POSITION_NO_1);

    if ((deocder != streamDecoder.end()) && deocder->second)
	{
        deocder->second->init();
        deocder->second->registerCallback(callback, userData);

		E_DjiLiveViewCameraSource cameraSource = source;

		if(m_cameraType < 0)
		{
			E_DjiCameraType cameraType;
			bool ret = getCameraType(DJI_MOUNT_POSITION_PAYLOAD_PORT_NO1, cameraType);
			if(ret)
			{
				m_cameraType = cameraType;
			}
		}

		if(m_cameraType == DJI_CAMERA_TYPE_UNKNOWN)
		{
			cameraSource = DJI_LIVEVIEW_CAMERA_SOURCE_DEFAULT;
		}
		else if(m_cameraType == DJI_CAMERA_TYPE_H30 || m_cameraType == DJI_CAMERA_TYPE_H30T)
		{
			E_DjiCameraManagerStreamSource streamSource = DJI_CAMERA_MANAGER_SOURCE_DEFAULT_CAM;

			if(source == DJI_LIVEVIEW_CAMERA_SOURCE_H20_WIDE)
			{
				streamSource = DJI_CAMERA_MANAGER_SOURCE_WIDE_CAM;
			}
			else if(source == DJI_LIVEVIEW_CAMERA_SOURCE_H20_ZOOM)
			{
				streamSource = DJI_CAMERA_MANAGER_SOURCE_ZOOM_CAM;
			}

			switchH30XStreamSource(DJI_MOUNT_POSITION_PAYLOAD_PORT_NO1, streamSource);
			cameraSource = DJI_LIVEVIEW_CAMERA_SOURCE_DEFAULT;
		}

        m_returnCode = DjiLiveview_StartH264Stream(DJI_LIVEVIEW_CAMERA_POSITION_NO_1, cameraSource,
                                           liveViewConvertH264ToRgbCallback);

		if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
		{
        	writeLogFile("wanter: %d startMainCameraStream errcode:0x%08llX\n", mode, m_returnCode);
			return false;
    	}

		m_startMainCameraStreamBoardModeFlag = mode;
		writeLogFile("wanter %d startMainCameraStream success\n", mode);
		return true;
    }

	writeLogFile("wanter: %d startMainCameraStream find DJI_LIVEVIEW_CAMERA_POSITION_NO_1 error\n", mode);
	return false;
}

bool djiFlightHandler::stopMainCameraStream(enum OnBoardModeType mode, E_DjiLiveViewCameraSource source)
{
	if(!m_startDjiLiveViewFlag)
	{
		writeLogFile("wanter:%d execute stopMainCameraStream, but startDjiLiveViewFlag is false\n", mode);
		return false;
	}

	if(mode != m_startMainCameraStreamBoardModeFlag)
	{
		writeLogFile("wanter:%d stopFpvCameraStream, but holder:%d\n", mode, m_startMainCameraStreamBoardModeFlag);
		return false;
	}

	E_DjiLiveViewCameraSource cameraSource = source;

	if(m_cameraType == DJI_CAMERA_TYPE_UNKNOWN || m_cameraType == DJI_CAMERA_TYPE_H30 || m_cameraType == DJI_CAMERA_TYPE_H30T)
	{
		cameraSource = DJI_LIVEVIEW_CAMERA_SOURCE_DEFAULT;
	}

	 m_returnCode = DjiLiveview_StopH264Stream(DJI_LIVEVIEW_CAMERA_POSITION_NO_1, cameraSource);
    if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		//sorry, I don't know why
		m_startMainCameraStreamBoardModeFlag = NoMode;
		writeLogFile("holder %d stopMainCameraStream errcode:0x%08llX\n", mode);
        return false;
    }

    auto deocder = streamDecoder.find(DJI_LIVEVIEW_CAMERA_POSITION_NO_1);
    if ((deocder != streamDecoder.end()) && deocder->second)
	{
        deocder->second->cleanup();
    }

	m_startMainCameraStreamBoardModeFlag = NoMode;
	writeLogFile("holder %d stopMainCameraStream success\n", mode);
	return true;
}

bool djiFlightHandler::startViceCameraStream(enum OnBoardModeType mode, CameraImageCallback callback, void *userData, E_DjiLiveViewCameraSource souce)
{
	if(!m_startDjiLiveViewFlag)
	{
		writeLogFile("wanter:%d execute startViceCameraStream, but startDjiLiveViewFlag is false\n", mode);
		return false;
	}

	if(NoMode != m_startViceCameraStreamBoardModeFlag)
	{
		writeLogFile("wanter:%d startViceCameraStream, but holder:%d\n", mode, m_startViceCameraStreamBoardModeFlag);
		return false;
	}

	auto deocder = streamDecoder.find(DJI_LIVEVIEW_CAMERA_POSITION_NO_2);

    if ((deocder != streamDecoder.end()) && deocder->second)
	{
        deocder->second->init();
        deocder->second->registerCallback(callback, userData);

        m_returnCode =  DjiLiveview_StartH264Stream(DJI_LIVEVIEW_CAMERA_POSITION_NO_2, DJI_LIVEVIEW_CAMERA_SOURCE_DEFAULT,
                                           liveViewConvertH264ToRgbCallback);

		if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
		{
        	writeLogFile("wanter: %d startViceCameraStream errcode:0x%08llX\n", mode, m_returnCode);
			return false;
    	}

		m_startViceCameraStreamBoardModeFlag = mode;
		writeLogFile("wanter: %d startViceCameraStream success\n", mode);
		return true;
    }

	writeLogFile("wanter: %d startViceCameraStream find DJI_LIVEVIEW_CAMERA_POSITION_NO_2 error\n", mode);
	return false;
}

bool djiFlightHandler::stopViceCameraStream(enum OnBoardModeType mode, E_DjiLiveViewCameraSource source)
{
	if(!m_startDjiLiveViewFlag)
	{
		writeLogFile("wanter:%d execute stopViceCameraStream, but startDjiLiveViewFlag is false\n", mode);
		return false;
	}

	if(mode != m_startViceCameraStreamBoardModeFlag)
	{
		writeLogFile("wanter:%d stopViceCameraStream, but holder:%d\n", mode, m_startViceCameraStreamBoardModeFlag);
		return false;
	}

	m_returnCode = DjiLiveview_StopH264Stream(DJI_LIVEVIEW_CAMERA_POSITION_NO_2, source);
    if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		//sorry, I don't know why
		m_startViceCameraStreamBoardModeFlag = NoMode;
		writeLogFile("holder:%d stopViceCameraStream errcode:0x%08llX\n", mode, m_returnCode);
        return false;
    }

    auto deocder = streamDecoder.find(DJI_LIVEVIEW_CAMERA_POSITION_NO_2);
    if ((deocder != streamDecoder.end()) && deocder->second)
	{
        deocder->second->cleanup();
    }

	m_startViceCameraStreamBoardModeFlag = NoMode;
	writeLogFile("holder:%d stopViceCameraStream success\n", mode);
	return true;
}

bool djiFlightHandler::startTopCameraStream(enum OnBoardModeType mode, CameraImageCallback callback, void *userData, E_DjiLiveViewCameraSource source)
{
	if(!m_startDjiLiveViewFlag)
	{
		writeLogFile("wanter:%d execute startTopCameraStream, but startDjiLiveViewFlag is false\n", mode);
		return false;
	}

	if(NoMode != m_startTopCameraStreamBoardModeFlag)
	{
		writeLogFile("wanter:%d startTopCameraStream, but holder:%d\n", mode, m_startTopCameraStreamBoardModeFlag);
		return false;
	}

	auto deocder = streamDecoder.find(DJI_LIVEVIEW_CAMERA_POSITION_NO_3);

    if ((deocder != streamDecoder.end()) && deocder->second)
	{
        deocder->second->init();
        deocder->second->registerCallback(callback, userData);

        m_returnCode =  DjiLiveview_StartH264Stream(DJI_LIVEVIEW_CAMERA_POSITION_NO_3, source,
                                           liveViewConvertH264ToRgbCallback);

		if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
		{
        	writeLogFile("wanter: %d startTopCameraStream errcode:0x%08llX\n", mode, m_returnCode);
			return false;
    	}

		m_startTopCameraStreamBoardModeFlag = mode;
		writeLogFile("wanter: %d startTopCameraStream success\n", mode);
		return true;
    }

	writeLogFile("wanter: %d startTopCameraStream find DJI_LIVEVIEW_CAMERA_POSITION_NO_3 error\n", mode);
	return false;
	
}

bool djiFlightHandler::stopTopCameraStream(enum OnBoardModeType mode, E_DjiLiveViewCameraSource source)
{
	if(!m_startDjiLiveViewFlag)
	{
		writeLogFile("wanter:%d execute stopTopCameraStream, but startDjiLiveViewFlag is false\n", mode);
		return false;
	}

	if(mode != m_startTopCameraStreamBoardModeFlag)
	{
		writeLogFile("wanter:%d stopTopCameraStream, but holder:%d\n", mode, m_startTopCameraStreamBoardModeFlag);
		return false;
	}

	m_returnCode = DjiLiveview_StopH264Stream(DJI_LIVEVIEW_CAMERA_POSITION_NO_3, source);
    if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		//sorry, I don't know why
		m_startTopCameraStreamBoardModeFlag = NoMode;
		writeLogFile("holder:%d stopTopCameraStream errcode:0x%08llX\n", mode, m_returnCode);
        return false;
    }

    auto deocder = streamDecoder.find(DJI_LIVEVIEW_CAMERA_POSITION_NO_3);
    if ((deocder != streamDecoder.end()) && deocder->second)
	{
        deocder->second->cleanup();
    }

	m_startTopCameraStreamBoardModeFlag = NoMode;
	writeLogFile("holder:%d stopTopCameraStream success\n", mode);
	return true;
}


bool djiFlightHandler::getStereoCameraParameters(T_DjiPerceptionCameraParametersPacket &packet)
{
	m_returnCode = DjiPerception_Init();
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("getStereoCameraParameters  DjiPerception_Init errcode:0x%08llX\n", m_returnCode);
		return false;
	}

	m_returnCode = DjiPerception_GetStereoCameraParameters(&packet);
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		DjiPerception_Deinit();
		writeLogFile("getStereoCameraParameters  DjiPerception_GetStereoCameraParameters errcode:0x%08llX\n", m_returnCode);
		return false;
	}

	for(int i=0; i<packet.directionNum; i++)
	{
		switch(packet.cameraParameters[i].direction)
		{
			case DJI_PERCEPTION_RECTIFY_DOWN:
			{
				writeLogFile("down StereoCameraParameters:\n");
				break;
			}

			case DJI_PERCEPTION_RECTIFY_FRONT:
			{
				writeLogFile("front StereoCameraParameters:\n");
				break;
			}

			case DJI_PERCEPTION_RECTIFY_REAR:
			{
				writeLogFile("back StereoCameraParameters:\n");
				break;
			}

			case DJI_PERCEPTION_RECTIFY_UP:
			{
				writeLogFile("up StereoCameraParameters:\n");
				break;
			}

			case DJI_PERCEPTION_RECTIFY_LEFT:
			{
				writeLogFile("left StereoCameraParameters:\n");
				break;
			}

			case DJI_PERCEPTION_RECTIFY_RIGHT:
			{
				writeLogFile("right StereoCameraParameters:\n");
				break;
			}

			default:
				break;
		}

		//printf parameter
		writeLogFile("leftIntrinsics: %f, %f, %f, %f, %f, %f, %f, %f, %f\n", 		packet.cameraParameters[i].leftIntrinsics[0],
																					packet.cameraParameters[i].leftIntrinsics[1],
																					packet.cameraParameters[i].leftIntrinsics[2],
																					packet.cameraParameters[i].leftIntrinsics[3],
																					packet.cameraParameters[i].leftIntrinsics[4],
																					packet.cameraParameters[i].leftIntrinsics[5],
																					packet.cameraParameters[i].leftIntrinsics[6],
																					packet.cameraParameters[i].leftIntrinsics[7],
																					packet.cameraParameters[i].leftIntrinsics[8]);

		writeLogFile("rightIntrinsics: %f, %f, %f, %f, %f, %f, %f, %f, %f\n", 		packet.cameraParameters[i].rightIntrinsics[0],
																					packet.cameraParameters[i].rightIntrinsics[1],
																					packet.cameraParameters[i].rightIntrinsics[2],
																					packet.cameraParameters[i].rightIntrinsics[3],
																					packet.cameraParameters[i].rightIntrinsics[4],
																					packet.cameraParameters[i].rightIntrinsics[5],
																					packet.cameraParameters[i].rightIntrinsics[6],
																					packet.cameraParameters[i].rightIntrinsics[7],
																					packet.cameraParameters[i].rightIntrinsics[8]);

		writeLogFile("rotationLeftInRight: %f, %f, %f, %f, %f, %f, %f, %f, %f\n", 	packet.cameraParameters[i].rotationLeftInRight[0],
																					packet.cameraParameters[i].rotationLeftInRight[1],
																					packet.cameraParameters[i].rotationLeftInRight[2],
																					packet.cameraParameters[i].rotationLeftInRight[3],
																					packet.cameraParameters[i].rotationLeftInRight[4],
																					packet.cameraParameters[i].rotationLeftInRight[5],
																					packet.cameraParameters[i].rotationLeftInRight[6],
																					packet.cameraParameters[i].rotationLeftInRight[7],
																					packet.cameraParameters[i].rotationLeftInRight[8]);

		writeLogFile("translationLeftInRight: %f, %f, %f\n", 						packet.cameraParameters[i].translationLeftInRight[0],
																					packet.cameraParameters[i].translationLeftInRight[1],
																					packet.cameraParameters[i].translationLeftInRight[2]);
	}

	m_returnCode = DjiPerception_Deinit();
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("getStereoCameraParameters  DjiPerception_Deinit errcode:0x%08llX\n", m_returnCode);
		return false;
	}

	return true;
}

bool djiFlightHandler::subscribePerceptionImage(E_DjiPerceptionDirection direction, DjiPerceptionImageCallback callback)
{
	m_returnCode = DjiPerception_Init();
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("subscribePerceptionImage  DjiPerception_Init errcode:0x%08llX\n", m_returnCode);
		return false;
	}

	//this will send a second data, about 10 pair image, then auto stop.
	m_returnCode = DjiPerception_SubscribePerceptionImage(direction, callback);

	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		DjiPerception_Deinit();
		writeLogFile("subscribePerceptionImage  DjiPerception_SubscribePerceptionImage errcode:0x%08llX\n", m_returnCode);
		return false;
	}

	m_returnCode = DjiPerception_Deinit();
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("subscribePerceptionImage  DjiPerception_Deinit errcode:0x%08llX\n", m_returnCode);
		return false;
	}

	return true;
}

bool djiFlightHandler::unsubscribePerceptionImage(E_DjiPerceptionDirection direction)
{
#if 0 // this is not correct mode
	m_returnCode = DjiPerception_Init();
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("unsubscribePerceptionImage  DjiPerception_Init errcode:0x%08llX\n", m_returnCode);
		return false;
	}

	m_returnCode = DjiPerception_UnsubscribePerceptionImage(direction);

	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		DjiPerception_Deinit();
		writeLogFile("unsubscribePerceptionImage  DjiPerception_SubscribePerceptionImage errcode:0x%08llX\n", m_returnCode);
		return false;
	}

	m_returnCode = DjiPerception_Deinit();
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("unsubscribePerceptionImage  DjiPerception_Deinit errcode:0x%08llX\n", m_returnCode);
		return false;
	}
#endif
	return true;
}


bool djiFlightHandler::gimbalManagerSetMode(E_DjiMountPosition mountPosition, E_DjiGimbalMode mode)
{
	m_returnCode = DjiGimbalManager_Init();

	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("gimbalManagerSetMode  DjiGimbalManager_Init errcode:0x%08llX\n", m_returnCode);
		return false;
	}

	m_returnCode = DjiGimbalManager_SetMode(mountPosition, mode);
    if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		DjiGimbalManager_Deinit();
        writeLogFile("gimbalManagerSetMode  DjiGimbalManager_SetMode errcode:0x%08llX\n", m_returnCode);
        return false;
    }

    m_returnCode = DjiGimbalManager_Deinit();
    if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("gimbalManagerSetMode  DjiGimbalManager_Deinit errcode:0x%08llX\n", m_returnCode);
        return false;
    }

	return true;
}

bool djiFlightHandler::gimbalManagerReset(E_DjiMountPosition mountPosition)
{
	m_returnCode = DjiGimbalManager_Init();

	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("gimbalManagerReset  DjiGimbalManager_Init errcode:0x%08llX\n", m_returnCode);
		return false;
	}

	m_returnCode = DjiGimbalManager_Reset(mountPosition, DJI_GIMBAL_RESET_MODE_PITCH_AND_YAW);
    if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		DjiGimbalManager_Deinit();
        writeLogFile("gimbalManagerReset  DjiGimbalManager_Reset errcode:0x%08llX\n", m_returnCode);
        return false;
    }

    m_returnCode = DjiGimbalManager_Deinit();
    if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("gimbalManagerReset  DjiGimbalManager_Deinit errcode:0x%08llX\n", m_returnCode);
        return false;
    }

	return true;
}


bool djiFlightHandler::gimbalManagerSetModeAndReset(E_DjiMountPosition mountPosition, E_DjiGimbalMode mode, E_DjiGimbalResetMode resetMode)
{
	m_returnCode = DjiGimbalManager_Init();

	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("gimbalManagerSetModeAndReset  DjiGimbalManager_Init errcode:0x%08llX\n", m_returnCode);
		return false;
	}

	//set mode
	m_returnCode = DjiGimbalManager_SetMode(mountPosition, mode);
    if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		DjiGimbalManager_Deinit();
        writeLogFile("gimbalManagerSetModeAndReset  DjiGimbalManager_SetMode errcode:0x%08llX\n", m_returnCode);
        return false;
    }

	//reset
	m_returnCode = DjiGimbalManager_Reset(mountPosition, resetMode);
    if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		DjiGimbalManager_Deinit();
        writeLogFile("gimbalManagerSetModeAndReset  DjiGimbalManager_Reset errcode:0x%08llX\n", m_returnCode);
        return false;
    }

    m_returnCode = DjiGimbalManager_Deinit();
    if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("gimbalManagerSetModeAndReset  DjiGimbalManager_Deinit errcode:0x%08llX\n", m_returnCode);
        return false;
    }

    sleepMs(2000);

	return true;
}


bool djiFlightHandler::gimbalManagerRotate(E_DjiMountPosition mountPosition, T_DjiGimbalManagerRotation rotation)
{
	m_returnCode = DjiGimbalManager_Init();

	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("gimbalManagerRotate  DjiGimbalManager_Init errcode:0x%08llX\n", m_returnCode);
		return false;
	}

	//set mode
	m_returnCode = DjiGimbalManager_SetMode(mountPosition, DJI_GIMBAL_MODE_YAW_FOLLOW);
    if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		DjiGimbalManager_Deinit();
        writeLogFile("gimbalManagerRotate  DjiGimbalManager_SetMode errcode:0x%08llX\n", m_returnCode);
        return false;
    }

	m_returnCode = DjiGimbalManager_Rotate(mountPosition, rotation);
    if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		DjiGimbalManager_Deinit();
        writeLogFile("gimbalManagerRotate  DjiGimbalManager_Rotate errcode:0x%08llX\n", m_returnCode);
        return false;
    }

    m_returnCode = DjiGimbalManager_Deinit();
    if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("gimbalManagerRotate  DjiGimbalManager_Deinit errcode:0x%08llX\n", m_returnCode);
        return false;
    }

	return true;
}


bool djiFlightHandler::gimbalManagerSetModeAndRestAndRotate(E_DjiMountPosition mountPosition, E_DjiGimbalMode mode, T_DjiGimbalManagerRotation rotation)
{
	m_returnCode = DjiGimbalManager_Init();

	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("gimbalManagerSetModeAndRestAndRotate  DjiGimbalManager_Init errcode:0x%08llX\n", m_returnCode);
		return false;
	}

	//set mode
	m_returnCode = DjiGimbalManager_SetMode(mountPosition, mode);
    if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		DjiGimbalManager_Deinit();
        writeLogFile("gimbalManagerSetModeAndRestAndRotate  DjiGimbalManager_SetMode errcode:0x%08llX\n", m_returnCode);
        return false;
    }

	//reset
	m_returnCode = DjiGimbalManager_Reset(mountPosition, DJI_GIMBAL_RESET_MODE_YAW);
    if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		DjiGimbalManager_Deinit();
        writeLogFile("gimbalManagerSetModeAndRestAndRotate  DjiGimbalManager_Reset errcode:0x%08llX\n", m_returnCode);
        return false;
    }

	sleepMs(2000);

	//rotate
	m_returnCode = DjiGimbalManager_Rotate(mountPosition, rotation);
	if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		DjiGimbalManager_Deinit();
		writeLogFile("gimbalManagerSetModeAndRestAndRotate  DjiGimbalManager_Rotate errcode:0x%08llX\n", m_returnCode);
		return false;
	}

    m_returnCode = DjiGimbalManager_Deinit();
    if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("gimbalManagerSetModeAndRestAndRotate  DjiGimbalManager_Deinit errcode:0x%08llX\n", m_returnCode);
        return false;
    }

	return true;
}


bool djiFlightHandler::gimbalManagersetModeAndRotate(E_DjiMountPosition mountPosition, E_DjiGimbalMode mode, T_DjiGimbalManagerRotation rotation)
{
	m_returnCode = DjiGimbalManager_Init();

	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("gimbalManagersetModeAndRotate	DjiGimbalManager_Init errcode:0x%08llX\n", m_returnCode);
		return false;
	}

	//set mode
	m_returnCode = DjiGimbalManager_SetMode(mountPosition, mode);
	if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		DjiGimbalManager_Deinit();
		writeLogFile("gimbalManagersetModeAndRotate	DjiGimbalManager_SetMode errcode:0x%08llX\n", m_returnCode);
		return false;
	}

	//rotate
	m_returnCode = DjiGimbalManager_Rotate(mountPosition, rotation);
	if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		DjiGimbalManager_Deinit();
		writeLogFile("gimbalManagersetModeAndRotate	DjiGimbalManager_Rotate errcode:0x%08llX\n", m_returnCode);
		return false;
	}

	m_returnCode = DjiGimbalManager_Deinit();
	if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("gimbalManagersetModeAndRotate	DjiGimbalManager_Deinit errcode:0x%08llX\n", m_returnCode);
		return false;
	}

	return true;
}


//Zoom refers to the elongation and shortening of the camera lens,
//focus refers to the slight movement of the lens left and right
bool djiFlightHandler::setCameraZoom(E_DjiMountPosition mountPosition, float multiplier)
{
	//Init camera manager module
	m_returnCode = DjiCameraManager_Init();
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("setCameraZoom DjiCameraManager_Init failed, error code: 0x%08X\n", m_returnCode);
		return false;
	}

	T_DjiCameraManagerOpticalZoomParam opticalZoomParam;

	m_returnCode = DjiCameraManager_GetOpticalZoomParam(mountPosition, &opticalZoomParam);
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		DjiCameraManager_DeInit();
		writeLogFile("setCameraZoom DjiCameraManager_GetOpticalZoomParam failed, error code: 0x%08X\n", m_returnCode);
		return false;
	}

	writeLogFile("currentOpticalZoomFactor,:%f,maxOpticalZoomFactor:%f\n", opticalZoomParam.currentOpticalZoomFactor, opticalZoomParam.maxOpticalZoomFactor);

	m_returnCode = DjiCameraManager_SetOpticalZoomParam(mountPosition, DJI_CAMERA_ZOOM_DIRECTION_OUT, fabs(multiplier));
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		DjiCameraManager_DeInit();
		writeLogFile("setCameraZoom DjiCameraManager_SetOpticalZoomParam failed, error code: 0x%08X\n", m_returnCode);
		return false;
	}

	m_returnCode = DjiCameraManager_DeInit();
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("setCameraZoom DjiCameraManager_DeInit failed, error code: 0x%08X\n", m_returnCode);
		return false;
	}

	return true;
}

//laser range is from  3m-1200m
bool djiFlightHandler::getLaserRangingInfo(E_DjiMountPosition position, float &distance)
{
	//Init camera manager module
	m_returnCode = DjiCameraManager_Init();
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("getLaserRangingInfo DjiCameraManager_Init failed, error code: 0x%08X\n", m_returnCode);
		return false;
	}

	T_DjiCameraManagerLaserRangingInfo rangingInfo = {0};

	m_returnCode = DjiCameraManager_GetLaserRangingInfo(position, &rangingInfo);
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		DjiCameraManager_DeInit();
		writeLogFile("getLaserRangingInfo DjiCameraManager_GetLaserRangingInfo failed, error code: 0x%08X\n", m_returnCode);
		return false;
	}

	distance = rangingInfo.distance * 0.1f;
	writeLogFile("getLaserRangingInfo distance:%f\n", distance);

	m_returnCode = DjiCameraManager_DeInit();
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("getLaserRangingInfo DjiCameraManager_DeInit failed, error code: 0x%08X\n", m_returnCode);
		return false;
	}

	return true;
}



bool djiFlightHandler::switchH30XStreamSource(E_DjiMountPosition position, E_DjiCameraManagerStreamSource streamSource)
{
	//Init camera manager module
	//DjiCameraManager_Init  execute time about 5s
	m_returnCode = DjiCameraManager_Init();
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("switchH30XStreamSource DjiCameraManager_Init failed, error code: 0x%08X\n", m_returnCode);
		return false;
	}

	m_returnCode = DjiCameraManager_SetStreamSource(position, streamSource);
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		DjiCameraManager_DeInit();
		writeLogFile("switchH30XStreamSource DjiCameraManager_SetStreamSource failed, error code: 0x%08X\n", m_returnCode);
		return false;
	}

	m_returnCode = DjiCameraManager_DeInit();
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("switchH30XStreamSource DjiCameraManager_DeInit failed, error code: 0x%08X\n", m_returnCode);
		return false;
	}

	return true;
}


bool djiFlightHandler::getCameraType(E_DjiMountPosition position, E_DjiCameraType &cameraType)
{
	//Init camera manager module
	//DjiCameraManager_Init  execute time about 5s
	m_returnCode = DjiCameraManager_Init();
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("getCameraType DjiCameraManager_Init failed, error code: 0x%08X\n", m_returnCode);
		return false;
	}

	m_returnCode = DjiCameraManager_GetCameraType(position, &cameraType);
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		DjiCameraManager_DeInit();
		writeLogFile("getCameraType DjiCameraManager_GetCameraType failed, error code: 0x%08X\n", m_returnCode);
		return false;
	}

	m_returnCode = DjiCameraManager_DeInit();
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("getCameraType DjiCameraManager_DeInit failed, error code: 0x%08X\n", m_returnCode);
		return false;
	}

	return true;
}

void djiFlightHandler::getCameraType()
{
	m_returnCode = DjiCameraManager_Init();
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("getCameraType DjiCameraManager_Init failed, error code: 0x%08X\n", m_returnCode);
		return;
	}

	E_DjiCameraType cameraType;

	m_returnCode = DjiCameraManager_GetCameraType(DJI_MOUNT_POSITION_PAYLOAD_PORT_NO1, &cameraType);
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("getCameraType 1 DjiCameraManager_GetCameraType failed, error code: 0x%08X\n", m_returnCode);
	}
	else
	{
		m_cameraType = cameraType;
	}

	m_returnCode = DjiCameraManager_GetCameraType(DJI_MOUNT_POSITION_PAYLOAD_PORT_NO2, &cameraType);
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("getCameraType 2 DjiCameraManager_GetCameraType failed, error code: 0x%08X\n", m_returnCode);
	}
	else
	{
		m_cameraType2 = cameraType;
	}

	m_returnCode = DjiCameraManager_DeInit();
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("getCameraType DjiCameraManager_DeInit failed, error code: 0x%08X\n", m_returnCode);
	}
}

bool djiFlightHandler::isDoubleGimbal()
{
	if(m_cameraType >= 0  && m_cameraType2 >=0)
	{
		return true;
	}

	return false;
}

void djiFlightHandler::setFlightMode(T_DjiFcSubscriptionDisplaymode *mode, const T_DjiDataTimestamp *timestamp)
{
	unique_lock<mutex> ulock(m_flightModeLock);
	memcpy(&m_flightMode, mode, sizeof(T_DjiFcSubscriptionDisplaymode));
	memcpy(&m_flightModeUpdateTime, timestamp, sizeof(T_DjiDataTimestamp));
	ulock.unlock();

	// 1s fflush once.
	fflush(m_logFp);
	writeLogFile("------------------------------------\n");
	writeLogFile("flight mode:%u\n", m_flightMode);
}

void djiFlightHandler::getFlightMode(T_DjiFcSubscriptionDisplaymode &mode, T_DjiDataTimestamp &timestamp)
{
	unique_lock<mutex> ulock(m_flightModeLock);
	memcpy(&mode, &m_flightMode, sizeof(T_DjiFcSubscriptionDisplaymode));
	memcpy(&timestamp, &m_flightModeUpdateTime, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
}

void djiFlightHandler::setFlightStatus(T_DjiFcSubscriptionFlightStatus *status, const T_DjiDataTimestamp *timestamp)
{
	unique_lock<mutex> ulock(m_flightStatusLock);
	memcpy(&m_flightStatus, status, sizeof(T_DjiFcSubscriptionFlightStatus));
	memcpy(&m_flightStatusUpdateTime, timestamp, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
	writeLogFile("flight status:%u\n", m_flightStatus);
}

void djiFlightHandler::getFlightStatus(T_DjiFcSubscriptionFlightStatus & status, T_DjiDataTimestamp &timestamp)
{
	unique_lock<mutex> ulock(m_flightStatusLock);
	memcpy(&status, &m_flightStatus, sizeof(T_DjiFcSubscriptionFlightStatus));
	memcpy(&timestamp, &m_flightStatusUpdateTime, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
}

void djiFlightHandler::setFlightVelocity(T_DjiFcSubscriptionVelocity *velocity, const T_DjiDataTimestamp *timestamp)
{
	unique_lock<mutex> ulock(m_flightVelocityLock);
	memcpy(&m_flightVelocity, velocity, sizeof(T_DjiFcSubscriptionVelocity));
	memcpy(&m_flightVelocityUpdateTime, timestamp, sizeof(T_DjiDataTimestamp));
	ulock.unlock();

	m_velocityLogCount++;
	if(m_velocityLogCount > 100)
	{
		m_velocityLogCount = 0;
		writeLogFile("velocity x: %lf, y:%lf, z:%lf, health :%u\n", m_flightVelocity.data.x,
							m_flightVelocity.data.y, m_flightVelocity.data.z, m_flightVelocity.health);
	}
}

void djiFlightHandler::getFlightVelocity(T_DjiFcSubscriptionVelocity &velocity, T_DjiDataTimestamp &timestamp)
{
	unique_lock<mutex> ulock(m_flightVelocityLock);
	memcpy(&velocity, &m_flightVelocity, sizeof(T_DjiFcSubscriptionVelocity));
	memcpy(&timestamp, &m_flightVelocityUpdateTime, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
}

void djiFlightHandler::setFlightPosition(T_DjiFcSubscriptionPositionFused *position, const T_DjiDataTimestamp *timestamp)
{
	unique_lock<mutex> ulock(m_flightPositionLock);
	memcpy(&m_flightPosition, position, sizeof(T_DjiFcSubscriptionPositionFused));
	memcpy(&m_flightPositionUpdateTime, timestamp, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
	writeLogFile("position long: %f, lat:%f, alt:%f, visible:%d\n", m_flightPosition.longitude * 180 / DJI_PI,
						m_flightPosition.latitude * 180 / DJI_PI, m_flightPosition.altitude, m_flightPosition.visibleSatelliteNumber);
}

void djiFlightHandler::getFlightPosition(T_DjiFcSubscriptionPositionFused &position, T_DjiDataTimestamp &timestamp)
{
	
	unique_lock<mutex> ulock(m_flightPositionLock);
	memcpy(&position, &m_flightPosition, sizeof(T_DjiFcSubscriptionPositionFused));
	memcpy(&timestamp, &m_flightPositionUpdateTime, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
}

void djiFlightHandler::setFlightRelativeHeight(T_DjiFcSubscriptionHeightRelative *height, const T_DjiDataTimestamp *timestamp)
{
	unique_lock<mutex> ulock(m_relativeHeightLock);
	memcpy(&m_relativeHeight, height, sizeof(T_DjiFcSubscriptionHeightRelative));
	memcpy(&m_flightRelativeHeightUpdateTime, timestamp, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
	writeLogFile("relative height:%f\n", m_relativeHeight);
}

void djiFlightHandler::getFlightRelativeHeight(T_DjiFcSubscriptionHeightRelative &height, T_DjiDataTimestamp &timestamp)
{
	unique_lock<mutex> ulock(m_relativeHeightLock);
	memcpy(&height, &m_relativeHeight, sizeof(T_DjiFcSubscriptionHeightRelative));
	memcpy(&timestamp, &m_flightRelativeHeightUpdateTime, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
}

void djiFlightHandler::setRtkConnectStatus(T_DjiFcSubscriptionRTKConnectStatus *status, const T_DjiDataTimestamp *timestamp)
{
	
	unique_lock<mutex> ulock(m_rtkConnectStatusLock);
	memcpy(&m_rtkConnectStatus, status, sizeof(T_DjiFcSubscriptionRTKConnectStatus));
	memcpy(&m_rtkConnectStatusUpdateTime, timestamp, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
	writeLogFile("rtkConnected:%d\n", m_rtkConnectStatus.rtkConnected);
}

void djiFlightHandler::getRtkConnectStatus(T_DjiFcSubscriptionRTKConnectStatus &status, T_DjiDataTimestamp &timestamp)
{
	unique_lock<mutex> ulock(m_rtkConnectStatusLock);
	memcpy(&status, &m_rtkConnectStatus, sizeof(T_DjiFcSubscriptionRTKConnectStatus));
	memcpy(&timestamp, &m_rtkConnectStatusUpdateTime, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
}

void djiFlightHandler::setRtkPositionInfo(T_DjiFcSubscriptionRtkPositionInfo *status, const T_DjiDataTimestamp *timestamp)
{
	unique_lock<mutex> ulock(m_rtkPositionInfoLock);
	memcpy(&m_rtkPositionInfo, status, sizeof(T_DjiFcSubscriptionRtkPositionInfo));
	memcpy(&m_rtkPoistionInfoUpdateTime, timestamp, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
	writeLogFile("rtkPositionInfo:%u\n", m_rtkPositionInfo);
}

void djiFlightHandler::getRtkPositionInfo(T_DjiFcSubscriptionRtkPositionInfo &status, T_DjiDataTimestamp &timestamp)
{
	unique_lock<mutex> ulock(m_rtkPositionInfoLock);
	memcpy(&status, &m_rtkPositionInfo, sizeof(T_DjiFcSubscriptionRtkPositionInfo));
	memcpy(&timestamp, &m_rtkPoistionInfoUpdateTime, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
}

void djiFlightHandler::setRtkYawInfo(T_DjiFcSubscriptionRtkYawInfo *status, const T_DjiDataTimestamp *timestamp)
{
	unique_lock<mutex> ulock(m_rtkYawInfoLock);
	memcpy(&m_rtkYawInfo, status, sizeof(T_DjiFcSubscriptionRtkYawInfo));
	memcpy(&m_rtkYawInfoUpdateTime, timestamp, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
	writeLogFile("rtkYawinfo:%u\n", m_rtkYawInfo);
}

void djiFlightHandler::getRtkYawInfo(T_DjiFcSubscriptionRtkYawInfo &status, T_DjiDataTimestamp &timestamp)
{
	unique_lock<mutex> ulock(m_rtkYawInfoLock);
	memcpy(&status, &m_rtkYawInfo, sizeof(T_DjiFcSubscriptionRtkYawInfo));
	memcpy(&timestamp, &m_rtkYawInfoUpdateTime, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
}

void djiFlightHandler::setFlightQuat(T_DjiFcSubscriptionQuaternion *quat, const T_DjiDataTimestamp *timestamp)
{
	unique_lock<mutex> ulock(m_FlightQuatLock);
	memcpy(&m_FlightQuat, quat, sizeof(T_DjiFcSubscriptionQuaternion));
	memcpy(&m_FlightQuatUpdateTime, timestamp, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
	

	double roll,pitch, yaw;

	getRollPitchYawFromQuaternion(m_FlightQuat, roll, pitch, yaw);

	m_quatLogCount++;
	if(m_quatLogCount > 100)
	{
		m_quatLogCount = 0;
		writeLogFile("roll:%lf, pitch:%lf, yaw:%lf\n", roll, pitch, yaw);
	}
}

void djiFlightHandler::getFlightQuat(T_DjiFcSubscriptionQuaternion &quat, T_DjiDataTimestamp &timestamp)
{
	unique_lock<mutex> ulock(m_FlightQuatLock);
	memcpy(&quat, &m_FlightQuat, sizeof(T_DjiFcSubscriptionQuaternion));
	memcpy(&timestamp, &m_FlightQuatUpdateTime, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
}


void djiFlightHandler::setRtkPosition(T_DjiFcSubscriptionRtkPosition * position, const T_DjiDataTimestamp *timestamp)
{
	unique_lock<mutex> ulock(m_rtkPositionLock);
	memcpy(&m_rtkPosition, position, sizeof(T_DjiFcSubscriptionRtkPosition));
	memcpy(&m_rtkPostionUpdateTime, timestamp, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
	writeLogFile("rtk postion long:%f, lat:%f, alt:%f\n", m_rtkPosition.longitude, m_rtkPosition.latitude, m_rtkPosition.hfsl);
}

void djiFlightHandler::getRtkPosition(T_DjiFcSubscriptionRtkPosition &position, T_DjiDataTimestamp &timestamp)
{
	unique_lock<mutex> ulock(m_rtkPositionLock);
	memcpy(&position, &m_rtkPosition, sizeof(T_DjiFcSubscriptionRtkPosition));
	memcpy(&timestamp, &m_rtkPostionUpdateTime, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
}

void djiFlightHandler::setGpsDetails(T_DjiFcSubscriptionGpsDetails *gpsStatus, const T_DjiDataTimestamp *timestamp)
{
	unique_lock<mutex> ulock(m_gpsDetailsLock);
	memcpy(&m_gpsDetails, gpsStatus, sizeof(T_DjiFcSubscriptionGpsDetails));
	memcpy(&m_gpsDetailsUpdateTime, timestamp, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
	writeLogFile("gpsDetails hdop:%f, fixState:%f, pdop:%f\n" , m_gpsDetails.hdop, m_gpsDetails.fixState,m_gpsDetails.pdop);
}

void djiFlightHandler::getGpsDetails(T_DjiFcSubscriptionGpsDetails &gpsStatus, T_DjiDataTimestamp &timestamp)
{
	unique_lock<mutex> ulock(m_gpsDetailsLock);
	memcpy(&gpsStatus, &m_gpsDetails, sizeof(T_DjiFcSubscriptionGpsDetails));
	memcpy(&timestamp, &m_gpsDetailsUpdateTime, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
}

void djiFlightHandler::setHeightFusion(T_DjiFcSubscriptionHeightFusion *height, const T_DjiDataTimestamp *timestamp)
{
	unique_lock<mutex> ulock(m_heightFusionLock);
	memcpy(&m_heightFusion, height, sizeof(T_DjiFcSubscriptionHeightFusion));
	memcpy(&m_heightFusionUpdateTime, timestamp, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
	writeLogFile("heightFusion:%f\n" , m_heightFusion);
}

void djiFlightHandler::getHeightFusion(T_DjiFcSubscriptionHeightFusion &height, T_DjiDataTimestamp &timestamp)
{
	unique_lock<mutex> ulock(m_heightFusionLock);
	memcpy(&height, &m_heightFusion, sizeof(T_DjiFcSubscriptionHeightFusion));
	memcpy(&timestamp, &m_heightFusionUpdateTime, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
}


void djiFlightHandler::setGimbalAngles(T_DjiFcSubscriptionGimbalAngles *pry, const T_DjiDataTimestamp *timestamp)
{
	unique_lock<mutex> ulock(m_gimbalAnglesPryLock);
	memcpy(&m_gimbalAnglesPry, pry, sizeof(T_DjiFcSubscriptionGimbalAngles));
	memcpy(&m_gimbalAnglesUpdateTime, timestamp, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
	m_gimbalAnglesLogCount++;
	if(m_gimbalAnglesLogCount>50)
	{
		m_gimbalAnglesLogCount = 0;
		writeLogFile("gimbal angles: pitch:%f, roll:%f, yaw:%f\n", m_gimbalAnglesPry.x, m_gimbalAnglesPry.y, m_gimbalAnglesPry.z);
	}
}

void djiFlightHandler::getGimbalAngles(T_DjiFcSubscriptionGimbalAngles &pry, T_DjiDataTimestamp &timestamp)
{
	unique_lock<mutex> ulock(m_gimbalAnglesPryLock);
	memcpy(&pry, &m_gimbalAnglesPry, sizeof(T_DjiFcSubscriptionGimbalAngles));
	memcpy(&timestamp, &m_gimbalAnglesUpdateTime, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
}



void djiFlightHandler::setGimbalStatus(T_DjiFcSubscriptionGimbalStatus *status, const T_DjiDataTimestamp *timestamp)
{
	unique_lock<mutex> ulock(m_gimbalStatusLock);
	memcpy(&m_gimbalStatus, status, sizeof(T_DjiFcSubscriptionGimbalStatus));
	memcpy(&m_gimbalStatusUpdateTime, timestamp, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
	writeLogFile("gimbal status mountStatus:%d, calibration:%d\n", m_gimbalStatus.mountStatus, m_gimbalStatus.prevCalibrationgResult);
}

void djiFlightHandler::getGimbalStatus(T_DjiFcSubscriptionGimbalStatus &status, T_DjiDataTimestamp &timestamp)
{
	unique_lock<mutex> ulock(m_gimbalStatusLock);
	memcpy(&status, &m_gimbalStatus, sizeof(T_DjiFcSubscriptionGimbalStatus));
	memcpy(&timestamp, &m_gimbalStatusUpdateTime, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
}



void djiFlightHandler::setGimbalControlMode(T_DjiFcSubscriptionGimbalControlMode *mode, const T_DjiDataTimestamp *timestamp)
{
	unique_lock<mutex> ulock(m_gimbalControlModeLock);
	memcpy(&m_gimbalControlMode, mode, sizeof(T_DjiFcSubscriptionGimbalControlMode));
	memcpy(&m_gimbalControlModeUpdateTime, timestamp, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
	writeLogFile("gimbal controlMode:%u\n", m_gimbalControlMode);
}

void djiFlightHandler::getGimbalControlMode(T_DjiFcSubscriptionGimbalControlMode &mode, T_DjiDataTimestamp &timestamp)
{
	unique_lock<mutex> ulock(m_gimbalControlModeLock);
	memcpy(&mode, &m_gimbalControlMode, sizeof(T_DjiFcSubscriptionGimbalControlMode));
	memcpy(&timestamp, &m_gimbalControlModeUpdateTime, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
}


void djiFlightHandler::setAltitudeFused(T_DjiFcSubscriptionAltitudeFused *alt, const T_DjiDataTimestamp *timestamp)
{
	unique_lock<mutex> ulock(m_altitudeFusedLock);
	memcpy(&m_altitudeFused, alt, sizeof(T_DjiFcSubscriptionAltitudeFused));
	memcpy(&m_altitudeFusedUpdateTime, timestamp, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
	writeLogFile("setAltitudeFused:%f\n", m_altitudeFused);
}

void djiFlightHandler::getAltitudeFused(T_DjiFcSubscriptionAltitudeFused &alt, T_DjiDataTimestamp &timestamp)
{
	unique_lock<mutex> ulock(m_altitudeFusedLock);
	memcpy(&alt, &m_altitudeFused, sizeof(T_DjiFcSubscriptionAltitudeFused));
	memcpy(&timestamp, &m_altitudeFusedUpdateTime, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
}


void djiFlightHandler::setAltitudeBarometer(T_DjiFcSubscriptionAltitudeBarometer *alt, const T_DjiDataTimestamp *timestamp)
{
	unique_lock<mutex> ulock(m_altitudeBarometerLock);
	memcpy(&m_altitudeBarometer, alt, sizeof(T_DjiFcSubscriptionAltitudeBarometer));
	memcpy(&m_altitudeBarometerUpdateTime, timestamp, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
	writeLogFile("setAltitudeBarometer:%f\n", m_altitudeBarometer);
}

void djiFlightHandler::getAltitudeBarometer(T_DjiFcSubscriptionAltitudeBarometer &alt, T_DjiDataTimestamp &timestamp)
{
	unique_lock<mutex> ulock(m_altitudeBarometerLock);
	memcpy(&alt, &m_altitudeBarometer, sizeof(T_DjiFcSubscriptionAltitudeBarometer));
	memcpy(&timestamp, &m_altitudeBarometerUpdateTime, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
}


void djiFlightHandler::setAltitudeHomePoint(T_DjiFcSubscriptionAltitudeOfHomePoint *alt, const T_DjiDataTimestamp *timestamp)
{
	unique_lock<mutex> ulock(m_altitudeHomePointLock);
	memcpy(&m_altitudeHomePoint, alt, sizeof(T_DjiFcSubscriptionAltitudeOfHomePoint));
	memcpy(&m_altitudeHomePointUpdateTime, timestamp, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
	writeLogFile("setAltitudeHomePoint:%f\n", m_altitudeHomePoint);
}

void djiFlightHandler::getAltitudeHomePoint(T_DjiFcSubscriptionAltitudeOfHomePoint &alt, T_DjiDataTimestamp &timestamp)
{
	unique_lock<mutex> ulock(m_altitudeHomePointLock);
	memcpy(&alt, &m_altitudeHomePoint, sizeof(T_DjiFcSubscriptionAltitudeOfHomePoint));
	memcpy(&timestamp, &m_altitudeHomePointUpdateTime, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
}


void djiFlightHandler::setBatteryInfo(T_DjiFcSubscriptionWholeBatteryInfo *batteryInfo, const T_DjiDataTimestamp *timestamp)
{
	unique_lock<mutex> ulock(m_batteryInfoLock);
	memcpy(&m_batteryInfo, batteryInfo, sizeof(T_DjiFcSubscriptionWholeBatteryInfo));
	memcpy(&m_batteryInfoUpdateTime, timestamp, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
	writeLogFile("battery info, capacity:%d mAh, voltage:%d mV, current:%d mA, percentage:%u % \n",
					m_batteryInfo.capacity, m_batteryInfo.voltage, m_batteryInfo.current, m_batteryInfo.percentage);
}

void djiFlightHandler::getBatteryInfo(T_DjiFcSubscriptionWholeBatteryInfo &batteryInfo, T_DjiDataTimestamp &timestamp)
{
	unique_lock<mutex> ulock(m_batteryInfoLock);
	memcpy(&batteryInfo, &m_batteryInfo, sizeof(T_DjiFcSubscriptionWholeBatteryInfo));
	memcpy(&timestamp, &m_batteryInfoUpdateTime, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
}

//avoid data
void djiFlightHandler::setAvoidData(T_DjiFcSubscriptionAvoidData *avoidData, const T_DjiDataTimestamp *timestamp)
{
	unique_lock<mutex> ulock(m_avoidDataLock);
	memcpy(&m_avoidData, avoidData, sizeof(T_DjiFcSubscriptionAvoidData));
	memcpy(&m_avoidDataUpdateTime, timestamp, sizeof(T_DjiDataTimestamp));
	ulock.unlock();

	m_avoidDataLogCount++;
	if(m_avoidDataLogCount > 50)
	{
		m_avoidDataLogCount = 0;
		writeLogFile("AvoidData down:%f, front:%f, right:%f, back:%f, left:%f, up:%f; health  down:%u, front:%u, right:%u, back:%u, left:%u, up:%u\n",
			m_avoidData.down, m_avoidData.front, m_avoidData.right, m_avoidData.back, m_avoidData.left, m_avoidData.up,
			m_avoidData.downHealth, m_avoidData.frontHealth, m_avoidData.rightHealth, m_avoidData.backHealth, m_avoidData.leftHealth, m_avoidData.upHealth);
	}
}

void djiFlightHandler::getAvoidData(T_DjiFcSubscriptionAvoidData &avoidData, T_DjiDataTimestamp &timestamp)
{
	unique_lock<mutex> ulock(m_avoidDataLock);
	memcpy(&avoidData, &m_avoidData, sizeof(T_DjiFcSubscriptionAvoidData));
	memcpy(&timestamp, &m_avoidDataUpdateTime, sizeof(T_DjiDataTimestamp));
	ulock.unlock();
}



void djiFlightHandler::getRollPitchYawFromQuaternion(T_DjiFcSubscriptionQuaternion &quat, double &roll, double &pitch, double &yaw)
{
	pitch = (dji_f64_t) asinf(-2 * quat.q1 * quat.q3 + 2 * quat.q0 * quat.q2) * 57.3;
 	roll = (dji_f64_t) atan2f(2 * quat.q2 * quat.q3 + 2 * quat.q0 * quat.q1,
                             -2 * quat.q1 * quat.q1 - 2 * quat.q2 * quat.q2 + 1) * 57.3;
 	yaw = (dji_f64_t) atan2f(2 * quat.q1 * quat.q2 + 2 * quat.q0 * quat.q3,
                             -2 * quat.q2 * quat.q2 - 2 * quat.q3 * quat.q3 + 1) * 57.3;

	pitch = wrap360(pitch);
	roll = wrap360(roll);
	yaw = wrap360(yaw);
}


double djiFlightHandler::wrap360(const double angle)
{
    double res = fmod(angle, 360.0);
    if (res < 0)
    {
        res += 360.0;
    }
    return res;
}


double djiFlightHandler::getCurrentYaw()
{
	double roll,pitch, yaw;

	getRollPitchYawFromQuaternion(m_FlightQuat, roll, pitch, yaw);

	return yaw;
}



bool djiFlightHandler::obtainJoyStickCtrlAuthority(enum OnBoardModeType mode)
{
	if(NoMode != m_obtainJoyStickCtrlAuthorityFlag)
	{
		//other mode are controling the flight in the onboardSystem
		writeLogFile("obtainJoyStickCtrlAuthority current holder:%d, wanter:%d\n", m_obtainJoyStickCtrlAuthorityFlag, mode);
		return false;
	}

	T_DjiFlightControllerRidInfo ridInfo = {0};

	ridInfo.latitude = 22.542812;
    ridInfo.longitude = 113.958902;
    ridInfo.altitude = 10;

	 m_returnCode = DjiFlightController_Init(ridInfo);
    if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
        writeLogFile("wanter:%d execute djiTest_flightControlInit failed, errcode:0x%08llX\n", mode, m_returnCode);
        return false;
    }

	m_returnCode = DjiFlightController_ObtainJoystickCtrlAuthority();
    if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("wanter:%d execute DjiFlightController_ObtainJoystickCtrlAuthority function failed,errcode:0x%08llX\n", mode, m_returnCode);
		return false  ;
    }

	writeLogFile("wanter:%d get obtainJoyStickCtrlAuthority success \n", mode);
	m_obtainJoyStickCtrlAuthorityFlag = mode;
	return true;
}

bool djiFlightHandler::releaseJoyStickCtrlAuthority(enum OnBoardModeType mode)
{
	if(mode != m_obtainJoyStickCtrlAuthorityFlag)
	{
		writeLogFile("mode %d ,you should not release the ctrl authority, current holder:%d!!!\n", mode, m_obtainJoyStickCtrlAuthorityFlag);
		return false;
	}

	m_obtainJoyStickCtrlAuthorityFlag = NoMode;

	m_returnCode = DjiFlightController_ReleaseJoystickCtrlAuthority();
    if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("holder %d release control authority failed, sorry, I do not know why, please talk to dji, errcode:0x%08llX\n", mode, m_returnCode);
		//sorry, I do not know  why.
        return false;
    }

	m_returnCode = DjiFlightController_DeInit();
    if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
        writeLogFile("holder: %d Deinit Flight Control sample failed,error code:0x%08llX\n",mode, m_returnCode);
		//sorry, I do not know  why.
        return false;
    }

	writeLogFile("holder %d release the control authority success\n", mode);

	return true;
}



bool djiFlightHandler::sendTakeOffCommandToCopter(enum OnBoardModeType mode)
{
	if(mode != m_obtainJoyStickCtrlAuthorityFlag)
	{
		writeLogFile("mode %d ,you don't have the ctrl authority, you should't send take off command, current holder:%d!!!\n",
																		mode, m_obtainJoyStickCtrlAuthorityFlag);
		return false;
	}
	m_returnCode = DjiFlightController_StartTakeoff();
    if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
        writeLogFile("holder %d send take off command to copter failed, errcode:0x%08llX\n", mode, m_returnCode);
        return false;
    }

	writeLogFile("holder %d send take off command to copter success\n", mode);
	return true;
}

bool djiFlightHandler::sendAutoLandCommandToCopter(enum OnBoardModeType mode)
{
	if(mode != m_obtainJoyStickCtrlAuthorityFlag)
	{
		writeLogFile("mode %d ,you don't have the ctrl authority, you should't send auto land to copter, current holder:%d!!!\n",
																		mode, m_obtainJoyStickCtrlAuthorityFlag);
		return false;
	}

	m_returnCode = DjiFlightController_StartLanding();
    if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("holder %d send auto land command to copter failed, errcode:0x%08llX\n", mode, m_returnCode);
        return false;
    }

	writeLogFile("holder %d send auto land command to copter success\n", mode);
	return true;
}


bool djiFlightHandler::sendAutoConfirmLandCommandToCopter(enum OnBoardModeType mode)
{
	if(mode != m_obtainJoyStickCtrlAuthorityFlag)
	{
		writeLogFile("mode %d ,you don't have the ctrl authority, you should't send auto confirm land to copter, current holder:%d!!!\n",
																		mode, m_obtainJoyStickCtrlAuthorityFlag);
		return false;
	}

	m_returnCode = DjiFlightController_StartConfirmLanding();
    if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("holder %d send auto confirm land command to copter failed, errcode:0x%08llX\n", mode, m_returnCode);
        return false;
    }

	writeLogFile("holder %d send auto confirm land command to copter success\n", mode);
	return true;
}


bool djiFlightHandler::sendAutoForceLandCommandToCopter(enum OnBoardModeType mode)
{
	if(mode != m_obtainJoyStickCtrlAuthorityFlag)
	{
		writeLogFile("mode %d ,you don't have the ctrl authority, you should't send auto force land to copter, current holder:%d!!!\n",
																		mode, m_obtainJoyStickCtrlAuthorityFlag);
		return false;
	}

	m_returnCode = DjiFlightController_StartForceLanding();
    if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("holder %d send auto force land command to copter failed, errcode:0x%08llX\n", mode, m_returnCode);
        return false;
    }

	writeLogFile("holder %d send auto force land command to copter success\n", mode);
	return true;
}


bool djiFlightHandler::sendLandPointToCopter(enum OnBoardModeType mode, T_DjiFlightControllerHomeLocation homeLocation)
{
	if(mode != m_obtainJoyStickCtrlAuthorityFlag)
	{
		writeLogFile("mode %d ,you don't have the ctrl authority, you should't send land point to copter, current holder:%d!!!\n",
																		mode, m_obtainJoyStickCtrlAuthorityFlag);
		return false;
	}

	m_returnCode = DjiFlightController_SetHomeLocationUsingGPSCoordinates(homeLocation);

	if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("holder %d send auto land point to copter failed, errcode:0x%08llX\n", mode, m_returnCode);
        return false;
    }

	writeLogFile("holder %d send auto land point to copter success\n", mode);
	return true;
}

bool djiFlightHandler::sendStartGoHomeToCopter(enum OnBoardModeType mode)
{
	if(mode != m_obtainJoyStickCtrlAuthorityFlag)
	{
		writeLogFile("mode %d ,you don't have the ctrl authority, you should't send start go home to copter, current holder:%d!!!\n",
																		mode, m_obtainJoyStickCtrlAuthorityFlag);
		return false;
	}

	m_returnCode = DjiFlightController_StartGoHome();

	if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("holder %d send start go home to copter failed, errcode:0x%08llX\n", mode, m_returnCode);
        return false;
    }

	writeLogFile("holder %d send start go home to copter success\n", mode);
	return true;
}


bool djiFlightHandler::sendCancelLandCommandtoCopter(enum OnBoardModeType mode)
{
	if(mode != m_obtainJoyStickCtrlAuthorityFlag)
	{
		writeLogFile("mode %d ,you don't have the ctrl authority, you should't send auto force land to copter, current holder:%d!!!\n",
																		mode, m_obtainJoyStickCtrlAuthorityFlag);
		return false;
	}

	m_returnCode = DjiFlightController_CancelLanding();

	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("holder %d send cance landing to copter failed, errcode:0x%08llX\n", mode, m_returnCode);
		return false;
	}

	writeLogFile("holder %d send cance landing to copter success\n", mode);
	return true;
}


void *yawControlThread(void *arg)
{
	T_DjiReturnCode returnCode;
	flightHandler.setYawControlFunctionFlag(true);
	flightHandler.writeLogFile("yawControlThread copter ration to targetAngle:%f\n", flightHandler.getTargetYawAngle());

	T_DjiFlightControllerJoystickMode joystickMode = {
        DJI_FLIGHT_CONTROLLER_HORIZONTAL_VELOCITY_CONTROL_MODE,
        DJI_FLIGHT_CONTROLLER_VERTICAL_VELOCITY_CONTROL_MODE,
        DJI_FLIGHT_CONTROLLER_YAW_ANGLE_RATE_CONTROL_MODE,
        DJI_FLIGHT_CONTROLLER_HORIZONTAL_GROUND_COORDINATE,
        DJI_FLIGHT_CONTROLLER_STABLE_CONTROL_MODE_ENABLE,
    };

	DjiFlightController_SetJoystickMode(joystickMode);

	T_DjiFlightControllerJoystickCommand joystickCommand = {0, 0, 0, 0};

	ACPid pid(0.1f, 0.001f, 0.0005f, 1000, AC_PID_FILT_HZ_DEFAULT, 0.01f);

	double error;
    float controlP, controlI, controlD;

	double targetAngle = flightHandler.getTargetYawAngle();

	
	while(flightHandler.getYawControlFunctionFlag())
	{
		error = targetAngle - flightHandler.getCurrentYaw();

		error = (float)fmod((float)fmod((error + 180), 360) + 360, 360) - 180;

		pid.setInputFilterAll(fabsf(error));
        controlP = pid.getP();
        controlI = pid.getI();
        controlD = pid.getD();

		if(error > 0)
		{
			joystickCommand.yaw = controlP + controlI + controlD;
		}
		else
		{
			joystickCommand.yaw = -(controlP + controlI + controlD);
		}

		if(fabsf(error) < 2.0f)
		{
			joystickCommand.yaw = 0;
			flightHandler.setYawControlFunctionFlag(false);
		}

		if(NoMode == flightHandler.getObtainJoyStickCtrlAuthorityFlag())
		{
			break;
		}

		returnCode = DjiFlightController_ExecuteJoystickAction(joystickCommand);

		if(returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
		{
			flightHandler.writeLogFile("yawControlThread DjiFlightController_ExecuteJoystickAction errcode:0x%08llX\n", returnCode);
			flightHandler.setYawControlFunctionFlag(false);
			return NULL;
		}

		flightHandler.sleepMs(2);
	}

	flightHandler.setYawControlFunctionFlag(false);

	return NULL;
}


bool djiFlightHandler::sendYawControlToCopter(enum OnBoardModeType mode, double angle)
{
	if(mode != m_obtainJoyStickCtrlAuthorityFlag)
	{
		writeLogFile("mode %d ,you don't have the ctrl authority, you should't send yaw control to copter, current holder:%d!!!\n",
																		mode, m_obtainJoyStickCtrlAuthorityFlag);
		return false;
	}

	int ret;

	if(m_yawControlFunctionFlag)
	{
		m_yawControlFunctionFlag = false;
		ret = pthread_join(m_yawControlTid, NULL);

		if(0 != ret)
		{
			writeLogFile("sendYawControlToCopter pthread_join err\n");
			return false;
		}
	}

	m_targetAngle = wrap360(angle);

	ret = pthread_create(&m_yawControlTid, NULL, yawControlThread, NULL);
	if(0 != ret)
	{
		writeLogFile("sendYawControlToCopter pthread_create err\n");
		m_yawControlFunctionFlag = false;
		return false;
	}

	return true;
}


bool djiFlightHandler::setFlightControlModeByVelocity(enum OnBoardModeType mode)
{
	
	if(mode != m_obtainJoyStickCtrlAuthorityFlag)
	{
			writeLogFile("mode %d ,you don't have the ctrl authority, you should't send setFlightControlModeByVelocity, current holder:%d!!!\n",
																			mode, m_obtainJoyStickCtrlAuthorityFlag);
			return false;
	}

	T_DjiFlightControllerJoystickMode joystickMode = {
        DJI_FLIGHT_CONTROLLER_HORIZONTAL_VELOCITY_CONTROL_MODE,
        DJI_FLIGHT_CONTROLLER_VERTICAL_VELOCITY_CONTROL_MODE,
        DJI_FLIGHT_CONTROLLER_YAW_ANGLE_RATE_CONTROL_MODE,
        DJI_FLIGHT_CONTROLLER_HORIZONTAL_GROUND_COORDINATE,
        DJI_FLIGHT_CONTROLLER_STABLE_CONTROL_MODE_ENABLE,
    };

	DjiFlightController_SetJoystickMode(joystickMode);

	return true;
}


double djiFlightHandler::getTargetYawAngle()
{
	return m_targetAngle;
}


bool djiFlightHandler::getYawControlFunctionFlag()
{
	return m_yawControlFunctionFlag;
}

void djiFlightHandler::setYawControlFunctionFlag( bool flag)
{
	m_yawControlFunctionFlag = flag;
}



void *velocityControlThread(void *arg)
{
	T_DjiReturnCode returnCode;
	flightHandler.setVelocityControlFunctionFlag(true);


	T_DjiFlightControllerJoystickMode joystickMode = {
        DJI_FLIGHT_CONTROLLER_HORIZONTAL_VELOCITY_CONTROL_MODE,
        DJI_FLIGHT_CONTROLLER_VERTICAL_VELOCITY_CONTROL_MODE,
        DJI_FLIGHT_CONTROLLER_YAW_ANGLE_RATE_CONTROL_MODE,
        DJI_FLIGHT_CONTROLLER_HORIZONTAL_GROUND_COORDINATE,
        DJI_FLIGHT_CONTROLLER_STABLE_CONTROL_MODE_ENABLE,
    };

	DjiFlightController_SetJoystickMode(joystickMode);

	uint32_t timeoutMs = flightHandler.getVelocityControlTimeoutMs();
	T_DjiVector3f velocity;
	flightHandler.getVelocityControlVelocity(velocity);

	double yawRate;
	flightHandler.getVelocitycontrolYawRate(yawRate);

	flightHandler.writeLogFile("velocityControlThread x:%f,y:%f,z:%f, rawRate:%f, timeoutMs:%d\n", velocity.x, velocity.y, velocity.z, yawRate, timeoutMs);

	T_DjiFlightControllerJoystickCommand joystickCommand = {velocity.x, velocity.y, velocity.z, (float)yawRate};

	uint32_t originTime = 0;
    uint32_t currentTime = 0;
    uint32_t elapsedTimeInMs = 0;

	flightHandler.getCurrentTimeMs(originTime);
	flightHandler.getCurrentTimeMs(currentTime);

	elapsedTimeInMs = currentTime - originTime;

	while (elapsedTimeInMs <= timeoutMs && flightHandler.getVelocityControlFunctionFlag())
	{
	 	if(NoMode == flightHandler.getObtainJoyStickCtrlAuthorityFlag())
		{
			break;
		}

  		returnCode = DjiFlightController_ExecuteJoystickAction(joystickCommand);

		if(returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
		{
			flightHandler.writeLogFile("velocityControlThread DjiFlightController_ExecuteJoystickAction errcode:0x%08llX\n", returnCode);
			flightHandler.setVelocityControlFunctionFlag(false);
			return NULL;
		}
		
		flightHandler.sleepMs(2);
        flightHandler.getCurrentTimeMs(currentTime);
        elapsedTimeInMs = currentTime - originTime;
	}

	flightHandler.setVelocityControlFunctionFlag(false);

	return NULL;
}


bool djiFlightHandler::sendVelocityControlToCopter(enum OnBoardModeType mode, T_DjiVector3f velocity, uint32_t timeoutMs, double yawRate)
{
	if(mode != m_obtainJoyStickCtrlAuthorityFlag)
	{
		writeLogFile("mode %d ,you don't have the ctrl authority, you should't send velocity control to copter, current holder:%d!!!\n",
																		mode, m_obtainJoyStickCtrlAuthorityFlag);
		return false;
	}

	memcpy(&m_velocityControlVelocity, &velocity, sizeof(T_DjiVector3f));
	m_velocitycontrolYawrate = yawRate;
	m_velocityControlTimeoutMs = timeoutMs;

	int ret;

	if(m_velocityControlFunctionFlag)
	{
		m_velocityControlFunctionFlag = false;
		ret = pthread_join(m_velocityControlTid, NULL);

		if(0 != ret)
		{
			writeLogFile("sendVelocityControlToCopter pthread_join err\n");
			return false;
		}
	}

	ret = pthread_create(&m_velocityControlTid, NULL, velocityControlThread, NULL);

	if(0 != ret)
	{
		writeLogFile("sendVelocityControlToCopter pthread_create err\n");
		m_velocityControlFunctionFlag = false;
		return false;
	}

	return true;
}


bool djiFlightHandler::velocityControlToCopter(enum OnBoardModeType mode, T_DjiVector3f velocity, double yawRate)
{
	if(mode != m_obtainJoyStickCtrlAuthorityFlag)
	{
		writeLogFile("mode %d ,you don't have the ctrl authority, you should't velocityControlToCopter, current holder:%d!!!\n",
																		mode, m_obtainJoyStickCtrlAuthorityFlag);
		return false;
	}

	T_DjiFlightControllerJoystickCommand joystickCommand = {velocity.x, velocity.y, velocity.z, (float)yawRate};


	m_returnCode = DjiFlightController_ExecuteJoystickAction(joystickCommand);

	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		flightHandler.writeLogFile("velocityControlToCopter errcode:0x%08llX\n", m_returnCode);
		return false;
	}

	return true;
}


bool djiFlightHandler::sendHorizontalVelocitySizeAndBearingControlToCopter(enum OnBoardModeType mode, float velocitySize, double velocityBearing, uint32_t timeoutMs)
{
	//north is 0 degree, east is 90 degree, clockwise rotation

	double bearing = wrap360(velocityBearing);

	T_DjiVector3f velocity = {0, 0, 0};

	// x is north, sin and cos need a radian
	velocity.x = velocitySize * cos(bearing * DJI_PI / 180);
	// y is east
	velocity.y = velocitySize * sin(bearing * DJI_PI / 180);

	return sendVelocityControlToCopter(mode, velocity, timeoutMs);
}


bool djiFlightHandler::controlCopterTurnOffMotors(enum OnBoardModeType mode)
{
	if(mode != m_obtainJoyStickCtrlAuthorityFlag)
	{
		writeLogFile("mode %d ,you don't have the ctrl authority, you should't turn off motors, current holder:%d!!!\n",
																		mode, m_obtainJoyStickCtrlAuthorityFlag);
		return false;
	}

	m_returnCode = DjiFlightController_TurnOffMotors();

	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("ControlCopterTurnOffMotors errcode:0x%08llX\n", m_returnCode);
		return false;
	}

	writeLogFile("mode:%d turn off motors successful\n", mode);

	return true;
}

bool djiFlightHandler::getVelocityControlFunctionFlag()
{
	return m_velocityControlFunctionFlag;
}

void djiFlightHandler::setVelocityControlFunctionFlag(bool flag)
{
	m_velocityControlFunctionFlag = flag;
}

uint32_t djiFlightHandler::getVelocityControlTimeoutMs()
{
	return m_velocityControlTimeoutMs;
}


void djiFlightHandler::getVelocityControlVelocity(T_DjiVector3f &velocity)
{
	memcpy(&velocity, &m_velocityControlVelocity, sizeof(T_DjiVector3f));
}

void djiFlightHandler::getVelocitycontrolYawRate(double &yawRate)
{
	yawRate = m_velocitycontrolYawrate;
}


void *positionControlThread(void *arg)
{
	T_DjiReturnCode returnCode;
	flightHandler.setPositionControlFunctionFlag(true);

	T_DjiFlightControllerJoystickMode joystickMode = {
		   DJI_FLIGHT_CONTROLLER_HORIZONTAL_POSITION_CONTROL_MODE,
		   DJI_FLIGHT_CONTROLLER_VERTICAL_POSITION_CONTROL_MODE,
		   DJI_FLIGHT_CONTROLLER_YAW_ANGLE_CONTROL_MODE,
		   DJI_FLIGHT_CONTROLLER_HORIZONTAL_GROUND_COORDINATE,
		   DJI_FLIGHT_CONTROLLER_STABLE_CONTROL_MODE_ENABLE,
	};

	DjiFlightController_SetJoystickMode(joystickMode);

	uint32_t timeoutMs = flightHandler.getPositionControlTimeoutMs();
	T_DjiVector3f position;
	flightHandler.getPositionControlPosition(position);

	flightHandler.writeLogFile("positionControlThread x:%f,y:%f,z:%f, timeoutMs:%d\n", position.x, position.y, position.z, timeoutMs);

	T_DjiFlightControllerJoystickCommand joystickCommand = {position.x, position.y, position.z, 0};

	uint32_t originTime = 0;
    uint32_t currentTime = 0;
    uint32_t elapsedTimeInMs = 0;

	flightHandler.getCurrentTimeMs(originTime);
	flightHandler.getCurrentTimeMs(currentTime);

	elapsedTimeInMs = currentTime - originTime;

	while(elapsedTimeInMs <= timeoutMs && flightHandler.getPositionControlFunctionFlag())
	{

		if(NoMode == flightHandler.getObtainJoyStickCtrlAuthorityFlag())
		{
			break;
		}

		returnCode = DjiFlightController_ExecuteJoystickAction(joystickCommand);

		if(returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
		{
			flightHandler.writeLogFile("positionControlThread DjiFlightController_ExecuteJoystickAction errcode:0x%08llX\n", returnCode);
			flightHandler.setVelocityControlFunctionFlag(false);
			return NULL;
		}

		flightHandler.sleepMs(2);
		flightHandler.getCurrentTimeMs(currentTime);
        elapsedTimeInMs = currentTime - originTime;
	}

	flightHandler.setPositionControlFunctionFlag(false);

	return NULL;
}


bool djiFlightHandler::sendPositionControlToCopter(enum OnBoardModeType mode, T_DjiVector3f position, uint32_t timeoutMs)
{
	if(mode != m_obtainJoyStickCtrlAuthorityFlag)
	{
		writeLogFile("mode %d ,you don't have the ctrl authority, you should't send position control to copter, current holder:%d!!!\n",
																		mode, m_obtainJoyStickCtrlAuthorityFlag);
		return false;
	}


	memcpy(&m_positionControlPosition, &position, sizeof(T_DjiVector3f));
	m_positionControlTimeoutMs = timeoutMs;

	int ret;

	if(m_positionControlFunctionFlag)
	{
		m_positionControlFunctionFlag = false;

		ret = pthread_join(m_positionControlTid, NULL);

		if(0 != ret)
		{
			writeLogFile("sendPositionControlToCopter pthread_join err\n");
			return false;
		}
	}


	ret = pthread_create(&m_positionControlTid, NULL, positionControlThread, NULL);

	if(0 != ret)
	{
		writeLogFile("sendPositionControlToCopter pthread_create err\n");
		m_positionControlFunctionFlag = false;
		return false;
	}

	return true;
}



bool djiFlightHandler::sendPositioncontrolToCopter(enum OnBoardModeType mode, T_DjiPositioningPosition position)
{
	//give long lat alt, to control copter move to position given;

	return true;
}




bool djiFlightHandler::getPositionControlFunctionFlag()
{
	return m_positionControlFunctionFlag;
}

void djiFlightHandler::setPositionControlFunctionFlag(bool flag)
{
	m_positionControlFunctionFlag = flag;
}

uint32_t djiFlightHandler::getPositionControlTimeoutMs()
{
	return m_positionControlTimeoutMs;
}

void djiFlightHandler::getPositionControlPosition(T_DjiVector3f &position)
{
	memcpy(&position, &m_positionControlPosition, sizeof(T_DjiVector3f));
}


enum OnBoardModeType djiFlightHandler::getObtainJoyStickCtrlAuthorityFlag()
{
	return m_obtainJoyStickCtrlAuthorityFlag;
}

static void *mopChannelSendNormalTask(void *arg)
{
	bool ret;
	flightHandler.setMopChannelSendNormalTaskRunFlag(true);
	flightHandler.sleepMs(MOP_CHANNEL_INIT_TIMEMS);

	while(flightHandler.getMopChannelSendNormalTaskRunFlag())
	{
		flightHandler.sleepMs(20);

		//send log,every time send 20 data for avoiding other message delays
		for(int i=0; i<20; i++)
		{
			if(!flightHandler.handleLogSend())
			{
				break;
			}
		}

		if (flightHandler.getMopChannelConnectedFlag() == false)
		{
			//clear buffer to avoid sending large amounts of data in the first time.
			flightHandler.clearMopChannelSendNormalBuffer();
			continue;
		}

		uint32_t needSendLen, retLen;
		uint8_t sendBuf[MOP_CHANNEL_BUFFER_SIZE];

		do
		{
			needSendLen = flightHandler.readMopChannelSendNormalBuffer(sendBuf, MOP_CHANNEL_BUFFER_SIZE);
			if(needSendLen <= 0)
			{
				break;
			}

			ret = flightHandler.mopChannelSendData(sendBuf, needSendLen, &retLen);
			if(!ret)
			{
				break;
			}
		}while(needSendLen == MOP_CHANNEL_BUFFER_SIZE);

	}

	return NULL;
}


static void *mopChannelRecvNormalTask(void *arg)
{
	bool ret;
	flightHandler.sleepMs(MOP_CHANNEL_INIT_TIMEMS);

	ret = flightHandler.mopChannelCreate();
	if(!ret)
	{
		return NULL;
	}

	 int cnt = 10;

	do
	{
		ret = flightHandler.mopChannelBind();
		if(ret)
		{
			//bind ip successfull
			break;
		}

	}while(cnt--);

	if(cnt <= 0)
	{
		flightHandler.writeLogFile("mopChannelRecvNormalTask mopChannelBind failed\n");
		return NULL;
	}

	flightHandler.setMopChannelRecvNormalTaskRunFlag(true);
	while(flightHandler.getMopChannelRecvNormalTaskRunFlag())
	{
		flightHandler.sleepMs(20);
		ret = flightHandler.mopChannelAccept();
		if(!ret)
		{
			//may be timeout
			continue;
		}

		flightHandler.setMopChannelConnectedFlag(true);
		while(flightHandler.getMopChannelConnectedFlag())
		{
			flightHandler.sleepMs(20);

			uint32_t retLen;
			uint8_t recvBuf[MOP_CHANNEL_BUFFER_SIZE];
			memset(recvBuf, 0, MOP_CHANNEL_BUFFER_SIZE);

			do
			{
				retLen = 0;
				ret = flightHandler.mopChannelRecvData(recvBuf, MOP_CHANNEL_BUFFER_SIZE, &retLen);
				if(!ret)
				{
					break;
				}

				if(retLen > 0)
				{
					flightHandler.writeMopChannelRecvNormalBuffer(&recvBuf[0], retLen);
				}
			}while(retLen == MOP_CHANNEL_BUFFER_SIZE);
		}
	}

	return NULL;
}


static void *mopChannelFileServiceAcceptTask(void *arg)
{
	bool ret;
	ret = flightHandler.fileMopChannelCreate();

	if(!ret)
	{
		return NULL;
	}

	 int cnt = 10;

	 do
	{
		ret = flightHandler.fileMopChannelBind();
		if(ret)
		{
			//bind ip successfull
			break;
		}

	}while(cnt--);

	 if(cnt <= 0)
	{
		flightHandler.writeLogFile("mopChannelFileServiceAcceptTask fileMopChannelBind failed\n");
		return NULL;
	}

	//while(1) handler accept
	flightHandler.fileMopChannelAcceptHandle();

	return NULL;
}


bool djiFlightHandler::mopChannelStartService()
{
	int ret;

	m_mopChannelConnectedFlag = false;
	m_flileMopChannelConnectedFlag = false;

	m_returnCode = DjiMopChannel_Init();
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("mopChannelStartService DjiMopChannel_Init errcode:0x%08llX\n", m_returnCode);
		return false;
	}

	//init sendbuff and revbuff
	ret = initCircularBuf(&m_mopChannelSendBuffer, CIRCULAR_BUFFER_SIZE);
	if(0 != ret)
	{
		writeLogFile("mopChannelStartService initCircularBuf m_mopChannelSendBuffer error\n");
		return false;
	}

	ret = initCircularBuf(&m_mopChannelRecvBuffer, CIRCULAR_BUFFER_SIZE);
	if(0 != ret)
	{
		writeLogFile("mopChannelStartService initCircularBuf m_mopChannelRecvBuffer error\n");
		return false;
	}

	m_returnCode = m_osalHandler->TaskCreate("sendTask", mopChannelSendNormalTask,
                                         MOP_CHANNEL_TASK_STACK_SIZE, NULL, &m_mopChannelNormalSendTask);
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("mopChannelStartService TaskCreate mopChannelSendNormalTask errcode:0x%08llX\n", m_returnCode);
		return false;
	}

	m_returnCode = m_osalHandler->TaskCreate("recvTask", mopChannelRecvNormalTask,
                                         MOP_CHANNEL_TASK_STACK_SIZE, NULL, &m_mopChannelNormalRecvTask);
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("mopChannelStartService TaskCreate mopChannelRecvNormalTask errcode:0x%08llX\n", m_returnCode);
		return false;
	}

	m_returnCode = m_osalHandler->TaskCreate("fileTask", mopChannelFileServiceAcceptTask,
                                         MOP_CHANNEL_TASK_STACK_SIZE, NULL, &m_fileServiceMopChannelAcceptTask);

	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("mopChannelStartService TaskCreate mopChannelFileServiceAcceptTask errcode:0x%08llX\n", m_returnCode);
		return false;
	}

	m_mopChannelStartServiceFlag = true;

	return true;
}


bool djiFlightHandler::mopChannelCreate()
{
	m_returnCode = DjiMopChannel_Create(&m_mopChannelRecvNormalHandle, DJI_MOP_CHANNEL_TRANS_UNRELIABLE);

	if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("mopChannelCreate errcode:0x%08llX\n", m_returnCode);
		return false;
	}

	return true;
}

bool djiFlightHandler::mopChannelBind()
{
	m_returnCode = DjiMopChannel_Bind(m_mopChannelRecvNormalHandle, MOP_CHANNEL_NORMAL_TRANSFOR_CHANNEL_ID);
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("mopChannelBind errcode:0x%08llX\n", m_returnCode);
		return false;
	}

	return true;
}

bool djiFlightHandler::mopChannelAccept()
{
	m_returnCode = DjiMopChannel_Accept(m_mopChannelRecvNormalHandle, &m_mopChannelNormalOutHandle);
	if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("mopChannelAccept errcode:0x%08llX\n", m_returnCode);
		//may be timeout
		return false;
	}

	return true;
}

bool djiFlightHandler::mopChannelRecvData(uint8_t *data, uint32_t len, uint32_t *realLen)
{
	m_returnCode = DjiMopChannel_RecvData(m_mopChannelNormalOutHandle, data, len, realLen);

	if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("mopChannelRecvData  code:0x%08llX \n", m_returnCode);
		if (m_returnCode == DJI_ERROR_MOP_CHANNEL_MODULE_CODE_CONNECTION_CLOSE)
		{
			m_mopChannelConnectedFlag = false;
			sleepMs(100);
			DjiMopChannel_Close(m_mopChannelNormalOutHandle);
			//destroy = recreate ?
			DjiMopChannel_Destroy(m_mopChannelNormalOutHandle);
		}
		return false;
	}

	return true;
}

bool djiFlightHandler::mopChannelSendData(uint8_t *data, uint32_t len, uint32_t *realLen)
{
	m_returnCode = DjiMopChannel_SendData(m_mopChannelNormalOutHandle, data, len, realLen);

	if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		m_mopChannelConnectedFlag = false;
		writeLogFile("mopChannelSendData code:0x%08llX \n", m_returnCode);
		return false;
	}

	return true;
}


bool djiFlightHandler::fileMopChannelCreate()
{
	m_returnCode = DjiMopChannel_Create(&m_fileServiceMopChannelHandle, DJI_MOP_CHANNEL_TRANS_RELIABLE);

	if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("fileMopChannelCreate errcode:0x%08llX\n", m_returnCode);
		return false;
	}

	return true;
}

bool djiFlightHandler::fileMopChannelBind()
{
	m_returnCode = DjiMopChannel_Bind(m_fileServiceMopChannelHandle, MOP_CHANNEL_FILE_SERVICE_CHANNEL_ID);

	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("fileMopChannelBind errcode:0x%08llX\n", m_returnCode);
		return false;
	}

	return true;
}


static void *mopChannelFileServiceRecvTask(void *arg)
{
	uint8_t clientNum = *(uint8_t *) arg;
    uint32_t recvRealLen;
    uint8_t *recvBuf;
    MD5_CTX uploadFileMd5Ctx;
    FILE *uploadFile = NULL;
    uint8_t uploadFileMd5[DJI_MD5_BUFFER_LEN] = {0};
    uint32_t uploadFileTotalSize = 0;
    int32_t uploadWriteLen;
    T_DjiMopChannel_FileInfo uploadFileInfo = {0};

	recvBuf = (uint8_t*)malloc(MOP_CHANNEL_FILE_SERVICE_RECV_BUFFER);

	if(recvBuf == NULL)
	{
		flightHandler.writeLogFile("mopChannelFileServiceRecvTask malloc error\n");
		return NULL;
	}

	T_MopFileServiceClientContent fileServiceContent = flightHandler.getFileServiceContentHandler(clientNum);
	fileServiceContent.uploadState = MOP_FILE_SERVICE_UPLOAD_IDEL;
    fileServiceContent.downloadState = MOP_FILE_SERVICE_DOWNLOAD_IDEL;

	bool ret;
	while(1)
	{
		flightHandler.sleepMs(20);
		ret = flightHandler.fileMopChannelRecvData(clientNum, recvBuf, MOP_CHANNEL_FILE_SERVICE_RECV_BUFFER, &recvRealLen);
		if(!ret)
		{
			return NULL;
		}

		if (recvRealLen > 0)
		{
			T_DjiMopChannel_FileTransfor *fileTransfor = (T_DjiMopChannel_FileTransfor *) recvBuf;

			switch (fileTransfor->cmd)
			{
				case DJI_MOP_CHANNEL_FILE_TRANSFOR_CMD_REQUEST:
				{
					//msdk send a request to psdk, subcmd is the next step to tell the pask
					if(fileTransfor->subcmd == DJI_MOP_CHANNEL_FILE_TRANSFOR_SUBCMD_REQUEST_UPLOAD)
					{
						//the next step is upload
						fileServiceContent.uploadState = MOP_FILE_SERVICE_UPLOAD_REQUEST_START;
						fileServiceContent.uploadSeqNum = fileTransfor->seqNum;

						UtilMd5_Init(&uploadFileMd5Ctx);
						uploadFileTotalSize = 0;
					}
					else if(fileTransfor->subcmd == DJI_MOP_CHANNEL_FILE_TRANSFOR_SUBCMD_REQUEST_DOWNLOAD)
					{
						//the next step is download
						fileServiceContent.downloadState = MOP_FILE_SERVICE_DOWNLOAD_REQUEST_START;
						fileServiceContent.downloadSeqNum = fileTransfor->seqNum;
					}
					break;
				}
				case DJI_MOP_CHANNEL_FILE_TRANSFOR_CMD_FILE_DOWNLOAD_REQ:
				{
					//down file
					//firist:check filename is exist?
					vector<string> logFileName;
					flightHandler.viewLogFile(logFileName);
					bool fileIsExist = false;

					for(int i=0; i<logFileName.size(); i++)
					{
						if(strcmp(fileTransfor->data.dwonloadReq.fileName, logFileName[i].c_str()) == 0)
						{
							fileIsExist = true;
							break;
						}
					}

					if(fileIsExist)
					{
						fileServiceContent.downloadState = MOP_FILE_SERVICE_DOWNLOAD_FILE_INFO_SUCCESS;
						fileServiceContent.downloadSeqNum = fileTransfor->seqNum;
						strcpy(fileServiceContent.fileName, fileTransfor->data.dwonloadReq.fileName);
					}
					else
					{
						fileServiceContent.downloadState = MOP_FILE_SERVICE_DOWNLOAD_FILE_INFO_FAILED;
						fileServiceContent.downloadSeqNum = fileTransfor->seqNum;
					}
					break;
				}
				case DJI_MOP_CHANNEL_FILE_TRANSFOR_CMD_FILE_INFO:
				{
					//msdk tell psdk the upload file md5 crc
					uploadFileInfo.fileLength = fileTransfor->data.fileInfo.fileLength;
                    memcpy(uploadFileInfo.md5Buf, fileTransfor->data.fileInfo.md5Buf, sizeof(uploadFileInfo.md5Buf));
					if(uploadFile != NULL)
					{
						fclose(uploadFile);
					}

					uploadFile = fopen(fileTransfor->data.fileInfo.fileName, "wb");
					if(uploadFile == NULL)
					{
						return NULL;
					}

					fileServiceContent.uploadState = MOP_FILE_SERVICE_UPLOAD_FILE_INFO_SUCCESS;
					fileServiceContent.uploadSeqNum = fileTransfor->seqNum;
					break;
				}
				case DJI_MOP_CHANNEL_FILE_TRANSFOR_CMD_FILE_DATA:
				{
					//msdk send psdk upload file, psdk save those data.
					if(uploadFile == NULL)
					{
						return NULL;
					}

					if(fileTransfor->subcmd == DJI_MOP_CHANNEL_FILE_TRANSFOR_SUBCMD_FILE_DATA_NORMAL)
					{
						fileServiceContent.uploadState = MOP_FILE_SERVICE_UPLOAD_DATA_SENDING;
						fileServiceContent.uploadSeqNum = fileTransfor->seqNum;

						uploadWriteLen = fwrite(&recvBuf[UTIL_OFFSETOF(T_DjiMopChannel_FileTransfor, data)], 1, fileTransfor->dataLen, uploadFile);
						if(uploadWriteLen > 0)
						{
							 uploadFileTotalSize += uploadWriteLen;
							 UtilMd5_Update(&uploadFileMd5Ctx, &recvBuf[UTIL_OFFSETOF(T_DjiMopChannel_FileTransfor, data)], fileTransfor->dataLen);
						}
					}
					else if(fileTransfor->subcmd == DJI_MOP_CHANNEL_FILE_TRANSFOR_SUBCMD_FILE_DATA_END)
					{
						fileServiceContent.uploadState = MOP_FILE_SERVICE_UPLOAD_DATA_SENDING;
						fileServiceContent.uploadSeqNum = fileTransfor->seqNum;

						 uploadWriteLen = fwrite(&recvBuf[UTIL_OFFSETOF(T_DjiMopChannel_FileTransfor, data)], 1, fileTransfor->dataLen, uploadFile);
						 if(uploadWriteLen > 0)
						 {
							uploadFileTotalSize += uploadWriteLen;
							UtilMd5_Update(&uploadFileMd5Ctx, &recvBuf[UTIL_OFFSETOF(T_DjiMopChannel_FileTransfor, data)], fileTransfor->dataLen);
							UtilMd5_Final(&uploadFileMd5Ctx, uploadFileMd5);

							fclose(uploadFile);
							uploadFile = NULL;

							if (uploadFileInfo.fileLength == uploadFileTotalSize)
							{
								if (memcmp(uploadFileInfo.md5Buf, uploadFileMd5, sizeof(uploadFileMd5)) == 0)
								{
									fileServiceContent.uploadState = MOP_FILE_SERVICE_UPLOAD_FINISHED_SUCCESS;
									fileServiceContent.uploadSeqNum = fileTransfor->seqNum;
								}
								else
								{
									fileServiceContent.uploadState = MOP_FILE_SERVICE_UPLOAD_FINISHED_FAILED;
									fileServiceContent.uploadSeqNum = fileTransfor->seqNum;
								}
							}
							else
							{
								fileServiceContent.uploadState = MOP_FILE_SERVICE_UPLOAD_FINISHED_FAILED;
								fileServiceContent.uploadSeqNum = fileTransfor->seqNum;
							}
						 }
					}
					break;
				}
				case DJI_MOP_CHANNEL_FILE_TRANSFOR_CMD_RESULT:
				{
					//msdk tell psdk downlaod file result.
					if(fileTransfor->subcmd == DJI_MOP_CHANNEL_FILE_TRANSFOR_SUBCMD_RESULT_OK)
					{
						fileServiceContent.downloadState = MOP_FILE_SERVICE_DOWNLOAD_FINISHED_SUCCESS;
					}
					else
					{
						fileServiceContent.downloadState = MOP_FILE_SERVICE_DOWNLOAD_FINISHED_FAILED;
					}
					break;
				}
				case DJI_MOP_CHANNEL_FILE_TRANSFOR_CMD_STOP_REQUEST:
				{
					//msdk requst psdk to stop transfrom data.
					if(fileTransfor->subcmd == DJI_MOP_CHANNEL_FILE_TRANSFOR_SUBCMD_STOP_UPLOAD)
					{
						fileServiceContent.uploadState = MOP_FILE_SERVICE_UPLOAD_STOP;
					}
					else if(fileTransfor->subcmd == DJI_MOP_CHANNEL_FILE_TRANSFOR_SUBCMD_STOP_DOWNLOAD)
					{
						fileServiceContent.downloadState = MOP_FILE_SERVICE_DOWNLOAD_STOP;
					}
					break;
				}
				default:
					break;

			}
		}

	}
	return NULL;
}


static void *mopChannelFileServiceSendTask(void *arg)
{
	T_DjiReturnCode returnCode;
    uint8_t clientNum = *(uint8_t *) arg;
    uint32_t sendRealLen = 0;
    uint8_t *sendBuf;
    MD5_CTX downloadFileMd5Ctx;
    FILE *downloadFile = NULL;
    uint8_t downloadFileMd5[DJI_MD5_BUFFER_LEN] = {0};
    uint64_t downloadFileTotalSize = 0;
    uint64_t downloadWriteLen;
    uint16_t downloadPackCount = 0;
    T_DjiMopChannel_FileInfo downloadFileInfo = {0};
    T_DjiMopChannel_FileTransfor transforAck = {0};
    T_DjiMopChannel_FileTransfor fileData = {0};
    T_DjiMopChannel_FileTransfor fileInfo = {0};
    char tempPath[DJI_FILE_PATH_SIZE_MAX];

	sendBuf = (uint8_t*)malloc(MOP_CHANNEL_FILE_SERVICE_SEND_BUFFER);
	if(sendBuf == NULL)
	{
		return NULL;
	}

	T_MopFileServiceClientContent fileServiceContent = flightHandler.getFileServiceContentHandler(clientNum);

	while(1)
	{
		flightHandler.sleepMs(20);
		//msdk upload data to psdk
		switch(fileServiceContent.uploadState)
		{
			case MOP_FILE_SERVICE_UPLOAD_REQUEST_START:
			{
				//psdk respond msdk the DJI_MOP_CHANNEL_FILE_TRANSFOR_CMD_REQUEST msg
				transforAck.cmd = DJI_MOP_CHANNEL_FILE_TRANSFOR_CMD_ACK;
				transforAck.subcmd = DJI_MOP_CHANNEL_FILE_TRANSFOR_SUBCMD_ACK_OK;
				transforAck.seqNum = fileServiceContent.uploadSeqNum;
				transforAck.dataLen = 0;
				DjiMopChannel_SendData(fileServiceContent.clientHandle, (uint8_t * ) & transforAck,
												UTIL_OFFSETOF(T_DjiMopChannel_FileTransfor, data), &sendRealLen);
				fileServiceContent.uploadState = MOP_FILE_SERVICE_UPLOAD_IDEL;
				break;
			}
			case MOP_FILE_SERVICE_UPLOAD_FILE_INFO_SUCCESS:
			{
				//psdk respond msdk it can upload file.
				transforAck.cmd = DJI_MOP_CHANNEL_FILE_TRANSFOR_CMD_ACK;
				transforAck.subcmd = DJI_MOP_CHANNEL_FILE_TRANSFOR_SUBCMD_ACK_OK;
				transforAck.seqNum = fileServiceContent.uploadSeqNum;
				transforAck.dataLen = 0;
				DjiMopChannel_SendData(fileServiceContent.clientHandle, (uint8_t * ) & transforAck,
											UTIL_OFFSETOF(T_DjiMopChannel_FileTransfor, data), &sendRealLen);

				fileServiceContent.uploadState = MOP_FILE_SERVICE_UPLOAD_IDEL;
				break;
			}
			case MOP_FILE_SERVICE_UPLOAD_FILE_INFO_FAILED:
			{
				transforAck.cmd = DJI_MOP_CHANNEL_FILE_TRANSFOR_CMD_ACK;
                transforAck.subcmd = DJI_MOP_CHANNEL_FILE_TRANSFOR_SUBCMD_ACK_REJECTED;
                transforAck.seqNum = fileServiceContent.uploadSeqNum;
                transforAck.dataLen = 0;
				DjiMopChannel_SendData(fileServiceContent.clientHandle, (uint8_t * ) & transforAck,
											UTIL_OFFSETOF(T_DjiMopChannel_FileTransfor, data), &sendRealLen);
                fileServiceContent.uploadState = MOP_FILE_SERVICE_UPLOAD_IDEL;
				break;
			}
			case MOP_FILE_SERVICE_UPLOAD_FINISHED_SUCCESS:
			{
				//psdk tell msdk upload success.
				transforAck.cmd = DJI_MOP_CHANNEL_FILE_TRANSFOR_CMD_RESULT;
				transforAck.subcmd = DJI_MOP_CHANNEL_FILE_TRANSFOR_SUBCMD_RESULT_OK;
				transforAck.seqNum = fileServiceContent.uploadSeqNum++;
				transforAck.dataLen = 0;
				DjiMopChannel_SendData(fileServiceContent.clientHandle, (uint8_t * ) & transforAck,
													UTIL_OFFSETOF(T_DjiMopChannel_FileTransfor, data), &sendRealLen);
                fileServiceContent.uploadState = MOP_FILE_SERVICE_UPLOAD_IDEL;
				break;
			}
			case MOP_FILE_SERVICE_UPLOAD_FINISHED_FAILED:
			{
				//psdk tell msdk upload failed.
				transforAck.cmd = DJI_MOP_CHANNEL_FILE_TRANSFOR_CMD_RESULT;
                transforAck.subcmd = DJI_MOP_CHANNEL_FILE_TRANSFOR_SUBCMD_RESULT_FAILED;
                transforAck.seqNum = fileServiceContent.uploadSeqNum++;
                transforAck.dataLen = 0;
                DjiMopChannel_SendData(fileServiceContent.clientHandle, (uint8_t * ) & transforAck,
										UTIL_OFFSETOF(T_DjiMopChannel_FileTransfor, data),
                                       &sendRealLen);
                fileServiceContent.uploadState = MOP_FILE_SERVICE_UPLOAD_IDEL;
				break;
			}
			case MOP_FILE_SERVICE_UPLOAD_STOP:
			{
				//respond msdk stop to transform data for upload
				transforAck.cmd = DJI_MOP_CHANNEL_FILE_TRANSFOR_CMD_STOP_ACK;
                transforAck.subcmd = DJI_MOP_CHANNEL_FILE_TRANSFOR_SUBCMD_STOP_UPLOAD;
                transforAck.seqNum = fileServiceContent.uploadSeqNum++;
                transforAck.dataLen = 0;
                DjiMopChannel_SendData(fileServiceContent.clientHandle, (uint8_t *) &transforAck,
										UTIL_OFFSETOF(T_DjiMopChannel_FileTransfor, data), &sendRealLen);
                fileServiceContent.uploadState = MOP_FILE_SERVICE_UPLOAD_IDEL;
				break;
			}
			default:
                break;
		}

		//msdk downlaod from psdk
		switch (fileServiceContent.downloadState)
		{
			case MOP_FILE_SERVICE_DOWNLOAD_REQUEST_START:
			{
				//psdk respond the DJI_MOP_CHANNEL_FILE_TRANSFOR_CMD_REQUEST msg
				transforAck.cmd = DJI_MOP_CHANNEL_FILE_TRANSFOR_CMD_ACK;
                transforAck.subcmd = DJI_MOP_CHANNEL_FILE_TRANSFOR_SUBCMD_ACK_OK;
                transforAck.seqNum = fileServiceContent.downloadSeqNum;
                transforAck.dataLen = 0;
                DjiMopChannel_SendData(fileServiceContent.clientHandle, (uint8_t * ) & transforAck,
												UTIL_OFFSETOF(T_DjiMopChannel_FileTransfor, data), &sendRealLen);
                fileServiceContent.downloadState = MOP_FILE_SERVICE_DOWNLOAD_IDEL;
				break;
			}
			case MOP_FILE_SERVICE_DOWNLOAD_STOP:
			{
				//psdk respond msdk stop to transform data for download
				transforAck.cmd = DJI_MOP_CHANNEL_FILE_TRANSFOR_CMD_STOP_ACK;
                transforAck.subcmd = DJI_MOP_CHANNEL_FILE_TRANSFOR_SUBCMD_STOP_DOWNLOAD;
                transforAck.seqNum = fileServiceContent.uploadSeqNum++;
                transforAck.dataLen = 0;
                DjiMopChannel_SendData(fileServiceContent.clientHandle, (uint8_t *) &transforAck,
											UTIL_OFFSETOF(T_DjiMopChannel_FileTransfor, data), &sendRealLen);
                fileServiceContent.downloadState = MOP_FILE_SERVICE_DOWNLOAD_IDEL;
				break;
			}
			case MOP_FILE_SERVICE_DOWNLOAD_FILE_INFO_SUCCESS:
			{
				//psdk send download file md5 crc to msdk.
				UtilMd5_Init(&downloadFileMd5Ctx);

				if (downloadFile != NULL)
				{
                    fclose(downloadFile);
                }

				sprintf(tempPath, "/HbLogs/%s", fileServiceContent.fileName);
				downloadFile = fopen(tempPath, "rb");
				if(downloadFile == NULL)
				{
					return NULL;
				}

				downloadFileTotalSize = 0;
				while(1)
				{
					fseek(downloadFile, downloadFileTotalSize, SEEK_SET);
					downloadWriteLen = fread(sendBuf, 1, MOP_CHANNEL_FILE_SERVICE_SEND_BUFFER, downloadFile);

					if(downloadWriteLen > 0)
					{
						downloadFileTotalSize += downloadWriteLen;
						UtilMd5_Update(&downloadFileMd5Ctx, sendBuf, downloadWriteLen);
						if(downloadWriteLen < MOP_CHANNEL_FILE_SERVICE_SEND_BUFFER)
						{
							break;
						}
					}
				}

				UtilMd5_Final(&downloadFileMd5Ctx, downloadFileMd5);

				fileInfo.cmd = DJI_MOP_CHANNEL_FILE_TRANSFOR_CMD_FILE_INFO;
				fileInfo.subcmd = DJI_MOP_CHANNEL_FILE_TRANSFOR_SUBCMD_DOWNLOAD_REQUEST;
				fileInfo.seqNum = fileServiceContent.downloadSeqNum;
				fileInfo.dataLen = sizeof(fileInfo) - UTIL_OFFSETOF(T_DjiMopChannel_FileTransfor, data);

				fileInfo.data.fileInfo.isExist = true;
                downloadFileInfo.fileLength = downloadFileTotalSize;
                fileInfo.data.fileInfo.fileLength = downloadFileTotalSize;
				strcpy(fileInfo.data.fileInfo.fileName, fileServiceContent.fileName);
                memcpy(&fileInfo.data.fileInfo.md5Buf, &downloadFileMd5, sizeof(downloadFileMd5));

				DjiMopChannel_SendData(fileServiceContent.clientHandle, (uint8_t *) &fileInfo,
																sizeof(T_DjiMopChannel_FileTransfor), &sendRealLen);

				fileServiceContent.downloadState = MOP_FILE_SERVICE_DOWNLOAD_DATA_SENDING;
                downloadFileTotalSize = 0;
                downloadPackCount = 0;
				break;
			}
			case MOP_FILE_SERVICE_DOWNLOAD_DATA_SENDING:
			{
				//psdk send download file data to msdk.
				if(downloadFile == NULL)
				{
					break;
				}

				returnCode = fseek(downloadFile, downloadFileTotalSize, SEEK_SET);
				if(returnCode != 0)
				{
					break;
				}

				downloadWriteLen = fread(&sendBuf[UTIL_OFFSETOF(T_DjiMopChannel_FileTransfor, data)],
                                         1,
                                         (MOP_CHANNEL_FILE_SERVICE_SEND_BUFFER -
                                          UTIL_OFFSETOF(T_DjiMopChannel_FileTransfor, data)),
                                         downloadFile);

				if(downloadWriteLen > 0)
				{
					downloadFileTotalSize += downloadWriteLen;
					downloadPackCount++;

					fileData.cmd = DJI_MOP_CHANNEL_FILE_TRANSFOR_CMD_FILE_DATA;
					fileData.dataLen = downloadWriteLen;
					fileData.seqNum++;

					if(downloadWriteLen == (MOP_CHANNEL_FILE_SERVICE_SEND_BUFFER - UTIL_OFFSETOF(T_DjiMopChannel_FileTransfor, data)))
					{
						fileData.subcmd = DJI_MOP_CHANNEL_FILE_TRANSFOR_SUBCMD_FILE_DATA_NORMAL;
					}
					else
					{
						fileData.subcmd = DJI_MOP_CHANNEL_FILE_TRANSFOR_SUBCMD_FILE_DATA_END;
					}

					memcpy(sendBuf, &fileData, UTIL_OFFSETOF(T_DjiMopChannel_FileTransfor, data));
                    returnCode = DjiMopChannel_SendData(fileServiceContent.clientHandle, sendBuf,
										(downloadWriteLen + UTIL_OFFSETOF(T_DjiMopChannel_FileTransfor, data)), &sendRealLen);

					if(returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
					{
						if(returnCode == DJI_ERROR_MOP_CHANNEL_MODULE_CODE_CONNECTION_CLOSE)
						{
							break;
						}
					}
				}
				break;
			}
			default:
				break;
		}

	}

	return NULL;
}


bool djiFlightHandler::fileMopChannelAcceptHandle()
{
	uint8_t currentClientNum = 0;
	while(1)
	{
		flightHandler.sleepMs(20);
		m_returnCode = DjiMopChannel_Accept(m_fileServiceMopChannelHandle,
                                          &m_fileServiceContent[currentClientNum].clientHandle);

		if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
		{
			writeLogFile("fileMopChannelAcceptHandle DjiMopChannel_Accept errcode:0x%08llX\n", m_returnCode);
			//may be timeout
			continue;
		}


		flightHandler.setFileMopChannelConnectedFlag(true);

		//start a thread for file handle
		m_fileServiceContent[currentClientNum].index = currentClientNum;
        m_returnCode = m_osalHandler->TaskCreate("fileServiceRecvTask",
                                             mopChannelFileServiceRecvTask,
                                             MOP_CHANNEL_TASK_STACK_SIZE,
                                             &m_fileServiceContent[currentClientNum].index,
                                             &m_fileServiceContent[currentClientNum].clientRecvTask);

		if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
		{
			writeLogFile("fileMopChannelAcceptHandle TaskCreate fileServiceRecvTask errcode:0x%08llX\n", m_returnCode);
			return false;
		}

		m_returnCode = m_osalHandler->TaskCreate("fileServiceSendTask",
                                             mopChannelFileServiceSendTask,
                                             MOP_CHANNEL_TASK_STACK_SIZE,
                                             &m_fileServiceContent[currentClientNum].index,
                                             &m_fileServiceContent[currentClientNum].clientSendTask);

		if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
		{
			writeLogFile("fileMopChannelAcceptHandle TaskCreate  fileServiceSendTask errcode:0x%08llX\n", m_returnCode);
			return false;
		}

		currentClientNum++;
        if (currentClientNum > MOP_CHANNEL_FILE_SERVICE_CLIENT_MAX_SUPPORT_NUM)
		{
            currentClientNum = 0;
        }
	}

	return true;
}

bool djiFlightHandler::fileMopChannelRecvData(uint8_t clientNum, uint8_t *data, uint32_t len, uint32_t *realLen)
{
	m_returnCode = DjiMopChannel_RecvData(m_fileServiceContent[clientNum].clientHandle, data, len, realLen);

	if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("fileMopChannelRecvData clientNum:%d code:0x%08llX \n", clientNum, m_returnCode);
		if (m_returnCode == DJI_ERROR_MOP_CHANNEL_MODULE_CODE_CONNECTION_CLOSE)
		{
			m_flileMopChannelConnectedFlag = false;
			sleepMs(100);
			m_osalHandler->TaskDestroy(m_fileServiceContent[clientNum].clientRecvTask);
			DjiMopChannel_Close(m_fileServiceContent[clientNum].clientHandle);
			DjiMopChannel_Destroy(m_fileServiceContent[clientNum].clientHandle);
		}
		return false;
	}

	return true;
}

bool djiFlightHandler::fileMopChannelSendData(uint8_t clientNum, uint8_t *data, uint32_t len, uint32_t *realLen)
{
	m_returnCode = DjiMopChannel_SendData(m_fileServiceContent[clientNum].clientHandle, data, len, realLen);

	if (m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		m_flileMopChannelConnectedFlag = false;
		writeLogFile("mopChannelSendData code:0x%08llX \n", m_returnCode);
		return false;
	}

	return true;
}

bool djiFlightHandler::getMopChannelSendNormalTaskRunFlag()
{
	return m_mopChannelSendNormalTaskRunFlag;
}

void djiFlightHandler::setMopChannelSendNormalTaskRunFlag(bool flag)
{
	m_mopChannelSendNormalTaskRunFlag = flag;
}

bool djiFlightHandler::getMopChannelRecvNormalTaskRunFlag()
{
	return m_mopChannelRecvNormalTaskRunFlag;
}

void djiFlightHandler::setMopChannelRecvNormalTaskRunFlag(bool flag)
{
	m_mopChannelRecvNormalTaskRunFlag = flag;
}


void djiFlightHandler::writeMopChannelRecvNormalBuffer(unsigned char *pdata, unsigned int count)
{
	writeCircularBuf(&m_mopChannelRecvBuffer, pdata, count);
}

void djiFlightHandler::writeMopChannelSendNormalBuffer(unsigned char *pdata, unsigned int count)
{
	writeCircularBuf(&m_mopChannelSendBuffer, pdata, count);
}


unsigned int djiFlightHandler::readMopChannelRecvNormalBuffer(unsigned char *pdata, unsigned int len)
{
	return readCircularBuf(&m_mopChannelRecvBuffer, pdata, len);
}

unsigned int djiFlightHandler::readMopChannelSendNormalBuffer(unsigned char *pdata, unsigned int len)
{
	return readCircularBuf(&m_mopChannelSendBuffer, pdata, len);
}

void djiFlightHandler::clearMopChannelSendNormalBuffer()
{
	clearCircularBuf(&m_mopChannelSendBuffer);
}

void djiFlightHandler::clearMopChannelRecvNormalBuffer()
{
	clearCircularBuf(&m_mopChannelRecvBuffer);
}

unsigned int djiFlightHandler::getFreeCountMopChannelSendNormalBuffer()
{
	return getFreeCount(&m_mopChannelSendBuffer);
}

unsigned int djiFlightHandler::getFreeCountMopChannelRecvNormalBuffer()
{
	return getFreeCount(&m_mopChannelRecvBuffer);
}

unsigned int djiFlightHandler::getDataCountMopChannelSendNormalBuffer()
{
	return getDataCount(&m_mopChannelSendBuffer);
}
	
unsigned int djiFlightHandler::getDataCountMopChannelRecvNormalBuffer()
{
	return getDataCount(&m_mopChannelRecvBuffer);
}


bool djiFlightHandler::registerMopChannelRecvCallback(mopChannelRecvCallback function, enum OnBoardModeType mode)
{
	if(!m_mopChannelStartServiceFlag)
	{
		return false;
	}

	if(NULL == function)
	{
		writeLogFile("registerMopChannelRecvCallback mode %d function is null\n", mode);
		return false;
	}

	for(int i=0; i<m_mopChannelRecvCallBackModeType.size(); i++)
	{
		if(mode == m_mopChannelRecvCallBackModeType[i])
		{
			writeLogFile("registerMopChannelRecvCallback mode %d multiple registration\n", mode);
			return false;
		}
	}

	m_mopChannelRecvCallBackFunction.push_back(function);
	m_mopChannelRecvCallBackModeType.push_back(mode);

	return true;
}

bool djiFlightHandler::getMopChannelConnectedFlag()
{
	return m_mopChannelConnectedFlag;
}

void djiFlightHandler::setMopChannelConnectedFlag(bool flag)
{
	m_mopChannelConnectedFlag = flag;
}


bool djiFlightHandler::getFlileMopChannelConnectedFlag()
{
	return m_flileMopChannelConnectedFlag;
}

void djiFlightHandler::setFileMopChannelConnectedFlag(bool flag)
{
	m_flileMopChannelConnectedFlag = flag;
}


T_MopFileServiceClientContent &djiFlightHandler::getFileServiceContentHandler(uint8_t clientNum)
{
	return m_fileServiceContent[clientNum];
}

void djiFlightHandler::mopCommunicationWithMsdkRun()
{
	linkProtocolType msg;
	uint16_t count;
	uint8_t buffer[MOP_CHANNEL_BUFFER_SIZE];

	while(m_mopChannelStartServiceFlag)
	{
		sleepMs(100);

		do
		{
			count = readMopChannelRecvNormalBuffer(buffer, MOP_CHANNEL_BUFFER_SIZE);
			for(int i=0; i<count; i++)
			{
            	if(handleLinkProtocolData(&msg,buffer[i]))
				{
					int assistLandMsgIdex = -1;
					for(int j=0; j<m_mopChannelRecvCallBackModeType.size(); j++)
					{
						if(m_mopChannelRecvCallBackModeType[j] == AssistLandMode)
						{
							assistLandMsgIdex = j;
						}
					}

                	switch(msg.m_msgid)
					{
						case linkProtocolHeartBeat:
							getHeartBeatFromMsdk(msg);
							break;

						case linkProtocolAck:
							for(int n=0; n<m_mopChannelRecvCallBackFunction.size(); n++)
							{
								if(m_mopChannelRecvCallBackFunction[n] != NULL)
								{
									(*m_mopChannelRecvCallBackFunction[n])(msg);
								}
							}
							break;

						case linkProtocolSetLandPoint:
							setLandPoint(msg);
							break;

						//assistland msg
						case linkProtocolCancelLand:
							if(assistLandMsgIdex >= 0)
							{
								if(m_mopChannelRecvCallBackFunction[assistLandMsgIdex] != NULL)
								{
									(*m_mopChannelRecvCallBackFunction[assistLandMsgIdex])(msg);
								}
							}
							break;

						// handle log msg
						case linkProtocolLogRequestList:
						case linkProtocolLogRequestData:
						case linkProtocolLogErase:
						case linkProtocolLogRequestEnd:
							handleLogMessage(msg);
							break;

						//handle update firware msg
						case linkProtocolUpdateFirware:
						case linkProtocolUpdateFirwareData:
							handleUpdateFirwareMessage(msg);
							break;
						default:
							break;
					}

				}

			}

		}while(count == MOP_CHANNEL_BUFFER_SIZE);
	}
}


void djiFlightHandler::setAssistLandSystemStatus(enum assistLandSystemState status)
{
	m_assistLandSystemStatus = status;
}

void djiFlightHandler::setAssistAvoidSystemStatus(enum assistAvoidSystemState status)
{
	m_assistAvoidSystemStatus = status;
}


int djiFlightHandler::packageMessage(uint8_t *buf, uint8_t msgId,std::string msgData, uint16_t msgLen)
{
	if(buf == NULL)
	{
		return 0;
	}

	int ret = 0;
    ret = msgLen + 4;

	//head
    buf[0] = 0xfc;
    buf[1] = msgId;
    buf[2] = msgLen;
	if(0 != msgLen)
	{
		msgData.copy((char*)(&buf[3]), msgLen);
	}

	// init crc
    buf[ret-1] = 0;

    for(int i=1; i<ret-1; i++)
	{
        buf[ret-1] ^= buf[i];
    }

    return ret;
}


void djiFlightHandler::sendHeartBeatToMsdk()
{
	if(false == m_mopChannelConnectedFlag)
	{
		return;
	}

	//package to json format
	Json::Value  item;
	Json::Value arrayObj;

	item["assistLandSystemState"] = m_assistLandSystemStatus;
	item["assistAvoidSystemState"] = m_assistAvoidSystemStatus;
	item["landPointState"] = m_landPointOk;

	arrayObj.append(item);

	std::string out = arrayObj.toStyledString();

	if(out.length()>= LINK_PROTOCOL_DATA_BUFFER_SIZE)
	{
		writeLogFile("sendHeartBeatToMsdk string length is bigger than %d\n", LINK_PROTOCOL_DATA_BUFFER_SIZE);
		return;
	}

	uint8_t dataBuf[sizeof(linkProtocolType)];

	memset(dataBuf, 0, sizeof(linkProtocolType));

	int count = packageMessage(dataBuf, linkProtocolHeartBeat, out, out.length());

	writeMopChannelSendNormalBuffer(dataBuf, count);
}

void djiFlightHandler::sendCancelLandToMsdk()
{
	if(false == m_mopChannelConnectedFlag)
	{
		return;
	}

	//package to json format
	Json::Value  item;
	Json::Value arrayObj;

	item["status"] = linkProtocolMessageStatusOk;

	arrayObj.append(item);

	std::string out = arrayObj.toStyledString();

	if(out.length()>= LINK_PROTOCOL_DATA_BUFFER_SIZE)
	{
		writeLogFile("sendCancelLandToMsdk string length is bigger than %d\n", LINK_PROTOCOL_DATA_BUFFER_SIZE);
		return;
	}

	uint8_t dataBuf[sizeof(linkProtocolType)];

	memset(dataBuf, 0, sizeof(linkProtocolType));

	int count = packageMessage(dataBuf, linkProtocolCancelLand, out, out.length());

	writeMopChannelSendNormalBuffer(dataBuf, count);

	writeLogFile("sendCancelLandToMsdk\n");
}


void djiFlightHandler::getHeartBeatFromMsdk(linkProtocolType &msg)
{
	Json::Reader reader;
	Json::Value in;
	reader.parse((char*)msg.m_data, in);

	m_canLandFlag = (bool)in["canLanding"].asUInt();
	double latitude = in["lat"].asDouble();
	double longitude = in["lon"].asDouble();
	m_isTestMode = (bool)in["isTest"].asUInt();
	m_isFixBaseRTK = (bool)in["isFixBaseRTK"].asUInt();

	m_noHeartCount = 0;

	writeLogFile("getHeartBeatFromMsdk %s\n", (char*)msg.m_data);
	writeLogFile("getHeartBeatFromMsdk mode:%d, canLanding:%d, latitude:%.10f, longitude:%.10f\n",(int)m_isTestMode, (int)m_canLandFlag, latitude, longitude);

	if(false == m_landPointOk &&
		(!iszero(latitude) && !iszero(longitude)) &&
		(isfinite(latitude) && isfinite(longitude)))
	{
		m_landPointOk = true;
		m_LandPointLatitude = latitude;
		m_LandPointLongitude = longitude;
		writeLogFile("getHeartBeatFromMsdk-save canLanding:%d, latitude:%.10f, longitude:%.10f\n", (int)m_canLandFlag, latitude, longitude);
	}
}

bool djiFlightHandler::getIsFixBaseRTKStaus()
{
	return m_isFixBaseRTK;
}



bool djiFlightHandler::getLogDir()
{
	if (0 == access("./HbLogs", F_OK))
	{
		m_getLogDirFlag = true;
		return true;
	}
	else
	{
		int ret = mkdir("./HbLogs", 0777);

		if(0 == ret)
		{
			m_getLogDirFlag = true;
			return true;
		}
		else
		{
			m_getLogDirFlag = false;
			return false;
		}
	}

	return false;
}



bool djiFlightHandler::createLogFile()
{
	if(!m_getLogDirFlag)
	{
		return false;
	}

	for(int i=1; i<1000; i++)
	{
		char logName[30];
		memset(logName, 0, sizeof(logName) / sizeof(logName[0]));
		sprintf(logName,"./HbLogs/hb_logs_%d.txt", i);
		m_lastLogNum = i;
		if(0 == access(logName, F_OK))
		{
			continue;
		}
		else
		{
			if(999 == i)
			{
				if(0 == system("rm  ./HbLogs/*  -rf"))
				{
					m_logFp =  fopen("./HbLogs/hb_logs_1.txt", "ab+");
					if(NULL == m_logFp)
					{
						m_lastLogNum = 0;
						cout << "createlogFile failed1" << endl;
						return false;
					}
					m_lastLogNum = 1;
					return true;
				}
				else
				{
					m_lastLogNum--;
					cout << "rm ./HbLogs/* -rf failed" << endl;
					m_logFp = NULL;
					return false;
				}
			}
			else
			{
				m_logFp =  fopen(logName, "ab+");
				if(NULL == m_logFp)
				{
					m_lastLogNum--;
					cout << "createLogFile failed2" << endl;
					return false;
				}
				return true;
			}
		}
	}

	return false;
}

bool djiFlightHandler::writeLogFile(const char * format, ...)
{
	size_t size, n;
	char sbuf[SBUF_SIZE];

	if(NULL == m_logFp)
	{
		return false;
	}

	va_list args;
	va_start (args, format);
	n = vsnprintf (sbuf,SBUF_SIZE,format, args);
	va_end (args);

	size = fwrite(sbuf, 1, n, m_logFp);
    if (size != n) {
        fclose(m_logFp);
		m_logFp = NULL;
		cout << "writeLogFile fialed" << endl;
        return false;
    }

	return true;
}

bool djiFlightHandler::clearAllLogFile()
{
	if(m_logFp != NULL)
	{
		fclose(m_logFp);
		m_logFp = NULL;
	}

	if(0 == system("rm  ./HbLogs/*  -rf"))
	{
		m_lastLogNum = 0;
		//restart log
		createLogFile();
		return true;
	}
	cout << "clear all logFile failed!!!" << endl;
	return false;
}

bool djiFlightHandler::viewLogFile(vector<string> &logFileName)
{
	DIR* logDir = NULL;
	logDir = opendir("./HbLogs");

	if(NULL == logDir)
	{
		cout << "opendir ./HbLogs failed!!!" << endl;
		return false;
	}

	struct dirent* dirInfo;
	while((dirInfo = readdir(logDir)) != NULL)
	{
		if(strcmp(".", dirInfo->d_name) != 0 && strcmp("..", dirInfo->d_name) != 0)
		{
			logFileName.push_back(dirInfo->d_name);
		}
	}

	closedir(logDir);
	return true;
}



void djiFlightHandler::recodeCpuTemperature()
{
	FILE *file = fopen("/sys/class/thermal/thermal_zone0/temp", "r");
	if(NULL != file)
	{
		char buf[10];
		float temp;
		memset(buf, 0, 10);
		size_t len = fread(buf, 1, 10, file);
		if(len > 0)
		{
			temp  = atof(buf);
			writeLogFile("cpu temperature is %f\n", temp / 1000.0f);
		}
		fclose(file);
	}
}

void djiFlightHandler::handleLogMessage(linkProtocolType &msg)
{
	switch(msg.m_msgid)
	{
		case linkProtocolLogRequestList:
			handleLogRequestList(msg);
			break;
		case linkProtocolLogRequestData:
			handleLogRequestData(msg);
			break;
		case linkProtocolLogErase:
			handleLogRequestErase(msg);
			break;
		case linkProtocolLogRequestEnd:
			handleLogRequestEnd(msg);
			break;
		default:
			break;
	}
}


void djiFlightHandler::handleLogRequestList(linkProtocolType &msg)
{
	if(m_logSendingFlag == true)
	{
		cout << " handleLogRequestList log download in progress!" << endl;
		return;
	}

	Json::Reader reader;
	Json::Value in;
	reader.parse((char*)msg.m_data, in);

	logRequestListType packet;
	packet.m_start = (uint16_t)in["start"].asUInt();
	packet.m_end = (uint16_t)in["end"].asUInt();

	m_logListing = false;
	m_logSending = false;
	m_logNumLogs = m_lastLogNum;

	if(m_logNumLogs == 0)
	{
		m_logNextListEntry = 0;
		m_logLastListEntry = 0;
	}
	else
	{
		m_logNextListEntry = packet.m_start;
		m_logLastListEntry = packet.m_end;

		if(m_logLastListEntry > m_logNumLogs)
		{
			m_logLastListEntry = m_logNumLogs;
		}

		if(m_logNextListEntry < 0)
		{
			m_logNextListEntry = 0;
		}
	}

	m_logListing = true;
	m_logSendingFlag = true;
}

void djiFlightHandler::handleLogRequestData(linkProtocolType &msg)
{
	if(m_logSendingFlag == true)
	{
		cout << "handleLogRequestData log download in progress!" << endl;
		return;
	}

	Json::Reader reader;
	Json::Value in;
	reader.parse((char*)msg.m_data, in);

	logRequestDataType packet;
	packet.m_ofs = (uint32_t)in["ofs"].asUInt();
	packet.m_count = (uint32_t)in["count"].asUInt();
	packet.m_logId = (uint16_t)in["logId"].asUInt();

	m_logListing = false;

	if(!m_logSending || m_logNumData != packet.m_logId)
	{
		m_logSending = false;
		if(m_openReadFileFlag)
		{
			m_openReadFileFlag = false;
			closeReadFile();
		}

		uint16_t numLogs = m_lastLogNum;
		if(packet.m_logId > numLogs)
		{
			cout << "handleLogRequestData packet log id: "<< packet.m_logId << " is error!" << endl;
			return;
		}
		uint32_t fileSize;
		bool ret = getLogFileSize(packet.m_logId, fileSize);
		if(!ret)
		{
			cout << "handleLogRequestData get log file size failed!" << endl;
			return;
		}

		if(fileSize == 0)
		{
			cout << "handleLogRequestData file size is zero!" << endl;
			return;
		}

		m_logNumData = packet.m_logId;
		m_logDataSize = fileSize;
		m_logDataOffset = 0;

		//open log file by reading
		if(!openReadFile(packet.m_logId))
		{
			m_openReadFileFlag = false;
			cout << "handleLogRequestData open ReadFile error!" << endl;
			return;
		}

		m_openReadFileFlag = true;
	}

	m_logSending = true;
	m_logSendingFlag = true;
}

void djiFlightHandler::handleLogRequestErase(linkProtocolType &msg)
{
	clearAllLogFile();
}

void djiFlightHandler::handleLogRequestEnd(linkProtocolType &msg)
{
	m_logSending = false;
	m_logSendingFlag = false;

	if(m_openReadFileFlag)
	{
		m_openReadFileFlag = false;
		closeReadFile();
	}
}


bool djiFlightHandler::openReadFile(uint32_t logNum)
{
	if(logNum < 0)
	{
		cout << "openReadFile log numbers is:" << logNum << endl;
		return false;
	}

	if(m_readFd != NULL)
	{
		closeReadFile();
	}

	char logName[30];
	memset(logName, 0, sizeof(logName) / sizeof(logName[0]));
	sprintf(logName,"./HbLogs/hb_logs_%d.txt", logNum);	

	m_readFd =  fopen("./HbLogs/hb_logs_1.txt", "r");
	if(NULL == m_readFd)
	{
		cout << "openReadFile open " << logName << " failed!" << endl;
		return false;
	}

	return true;
}


void djiFlightHandler::closeReadFile()
{
	if(NULL != m_readFd)
	{
		fclose(m_readFd);
		free(m_readFd);
		m_readFd = NULL;
	}
}


int32_t djiFlightHandler::readLogData(char *buffer, int readSize)
{
	if(NULL == m_readFd || readSize <= 0)
	{
		cout << "readLogData readFd is null or readSize is smaller than zero." << endl;
		return -1;
	}

	size_t count;
	count = fread(buffer, readSize, 1, m_readFd);

	return count;
}


bool djiFlightHandler::getLogFileSize(uint32_t logNum, uint32_t &FileSize)
{
	FileSize = 0;
	if(logNum < 0)
	{
		cout << "getLogFileSize log numbers is:" << logNum << endl;
		return false;
	}

	char logName[30];
	memset(logName, 0, sizeof(logName) / sizeof(logName[0]));
	sprintf(logName,"./HbLogs/hb_logs_%d.txt", logNum);	

	struct stat fileStat;
	if(stat(logName, &fileStat) != 0)
	{
		cout << "getLogFileSize stat " << logName << " failed!" << endl;
		return false;
	}

	FileSize = fileStat.st_size;
	return true;
}

bool djiFlightHandler::handleLogSend()
{
	if(!m_logSendingFlag)
	{
		return false;
	}

	if(m_logListing)
	{
		return handleLogSendListing();
	}

	if(m_logSending)
	{
		return handleLogSendData();
	}

	return false;
}

bool djiFlightHandler::handleLogSendListing()
{
	uint32_t size;

	if(false == m_mopChannelConnectedFlag)
	{
		return false;
	}

	if(getFreeCountMopChannelSendNormalBuffer() < 300)
	{
		cout << "handleLogSendListing send buffer space < 300!" << endl;
		return false;
	}

	getLogFileSize(m_logNextListEntry, size);

	//packet to json
	Json::Value  item;
	Json::Value arrayObj;

	item["size"] = size;
	item["logId"] = m_logNextListEntry;
	item["numPosData"] = m_logNumLogs;
	item["lastPosDataNum"] = m_logLastListEntry;


	arrayObj.append(item);

	std::string out = arrayObj.toStyledString();

	if(out.length()>= LINK_PROTOCOL_DATA_BUFFER_SIZE)
	{
		writeLogFile("sendCancelLandToMsdk string length is bigger than %d\n", LINK_PROTOCOL_DATA_BUFFER_SIZE);
		return false;
	}

	uint8_t dataBuf[sizeof(linkProtocolType)];

	memset(dataBuf, 0, sizeof(linkProtocolType));

	int count = packageMessage(dataBuf, linkProtocolSendLogList, out, out.length());

	//send to send buffer
	writeMopChannelSendNormalBuffer(dataBuf, count);

	if(m_logNextListEntry == m_logLastListEntry)
	{
		m_logListing = false;
		m_logSendingFlag = false;
	}
	else
	{
		m_logNextListEntry++;
	}

	return true;
}


bool djiFlightHandler::handleLogSendData()
{
	int32_t ret = 0;

	if(false == m_mopChannelConnectedFlag)
	{
		return false;
	}

	if(!m_openReadFileFlag)
	{
		cout << "handleLogSendData open read file flag is false!" << endl;
		return false;
	}

	if(getFreeCountMopChannelSendNormalBuffer() < 300)
	{
		cout << "handleLogSendData send buffer space < 300!" << endl;
		return false;
	}

	char data[91] = {0};
	//here remain a byte for '\0'
	ret = readLogData(data, 90);

	if(ret < 0)
	{
		cout << "fread error or EOF read over." << endl;
		ret = 0;
	}

	//packet to json
	Json::Value  item;
	Json::Value arrayObj;

	item["ofs"] = m_logDataOffset;
	item["logId"] = m_logNumData;
	item["count"] = ret;
	item["data"] = data;

	arrayObj.append(item);

	std::string out = arrayObj.toStyledString();

	if(out.length()>= LINK_PROTOCOL_DATA_BUFFER_SIZE)
	{
		writeLogFile("sendCancelLandToMsdk string length is bigger than %d\n", LINK_PROTOCOL_DATA_BUFFER_SIZE);
		return false;
	}

	uint8_t dataBuf[sizeof(linkProtocolType)];

	memset(dataBuf, 0, sizeof(linkProtocolType));

	int count = packageMessage(dataBuf, linkProtocolSendLogData, out, out.length());

	//send to send buffer
	writeMopChannelSendNormalBuffer(dataBuf, count);

	m_logDataOffset += ret;
	if(ret < 90 || m_logDataOffset >= m_logDataSize)
	{
		m_logSending = false;
		m_logSendingFlag = false;
	}

	return true;
}

void djiFlightHandler::handleUpdateFirwareMessage(linkProtocolType &msg)
{
	switch(msg.m_msgid)
	{
		case linkProtocolUpdateFirware:
			handleUpdateFirware(msg);
			break;
		case linkProtocolUpdateFirwareData:
			handleUpdateFirwareData(msg);
			break;
		default:
			break;
	}
}

void djiFlightHandler::handleUpdateFirware(linkProtocolType &msg)
{
	Json::Reader reader;
	Json::Value in;
	reader.parse((char*)msg.m_data, in);

#if 0
	updateFirwareType packet;
	packet.m_msgId = (uint8_t)in["msgId"].asUInt();
	packet.m_dataSize = (uint16_t)in["dataSize"].asUInt();

	int stringLens = in["md5Crc"].asString().length();
	if(stringLens != 32)
	{
		writeLogFile("handleUpdateFirware  stringLens is %d ,not equal 32\n",stringLens);
		stringLens = stringLens < 32 ? stringLens : 32;
	}
	memcpy(packet.m_md5Crc, in["md5Crc"].asCString(), stringLens);
#else
	m_updateOffsetCount = 0;
	m_updateDataSize = in["dataSize"].asUInt();
	md5crc = in["md5Crc"].asCString();
#endif

	m_updateFp =  fopen("/usr/local/bin/update.bin", "wb+");

	if(m_updateFp != NULL)
	{
		sendUpdateFirwareState(updataFirwareStateRecvData);
		m_updateFlag = true;
	}
	else
	{
		writeLogFile("handleUpdateFirware open /usr/local/bin/update.bin failed\n");
		sendUpdateFirwareState(updataFirwareStateFailed);
		m_updateDataSize = 0;
		m_updateFlag = false;
	}
}

void djiFlightHandler::handleUpdateFirwareData(linkProtocolType &msg)
{
	if(m_updateFlag == false)
	{
		sendUpdateFirwareState(updataFirwareStateFailed);
		return;
	}

	if(m_updateFp == NULL)
	{
		sendUpdateFirwareState(updataFirwareStateFailed);
		m_updateFlag = false;
		m_updateOffsetCount = 0;
		m_updateDataSize = 0;
		return;
	}

	Json::Reader reader;
	Json::Value in;
	reader.parse((char*)msg.m_data, in);

#if 0
	updateFirwareDataType packet;
	packet.m_msgId = (uint8_t)in["msgId"].asUInt();
	packet.m_ofs = (uint32_t)in["ofs"].asUInt();
	packet.m_count = (uint32_t)in["count"].asUInt();
	packet.m_count = packet.m_count <= 90 ? packet.m_count : 90;
	memcpy(packet.m_data, in["data"].asCString(), packet.m_count);
#else
	int offset = (uint32_t)in["ofs"].asUInt();
	int count = (uint32_t)in["count"].asUInt();

#endif
}


void djiFlightHandler::sendUpdateFirwareState(enum updataFirwareState state)
{
	if(false == m_mopChannelConnectedFlag)
	{
		return;
	}

	//package to json format
	Json::Value  item;
	Json::Value arrayObj;

	item["status"] = state;

	arrayObj.append(item);

	std::string out = arrayObj.toStyledString();

	if(out.length()>= LINK_PROTOCOL_DATA_BUFFER_SIZE)
	{
		writeLogFile("sendUpdateFirwareState string length is bigger than %d\n", LINK_PROTOCOL_DATA_BUFFER_SIZE);
		return;
	}

	uint8_t dataBuf[sizeof(linkProtocolType)];

	memset(dataBuf, 0, sizeof(linkProtocolType));

	int count = packageMessage(dataBuf, linkProtocolUpdateFirwareState, out, out.length());

	writeMopChannelSendNormalBuffer(dataBuf, count);

	writeLogFile("sendUpdateFirwareState\n");

}

void djiFlightHandler::setLandPoint(linkProtocolType &msg)
{
	Json::Reader reader;
	Json::Value in;
	reader.parse((char*)msg.m_data, in);

	uint32_t latitude = (uint32_t)in["latitude"].asUInt();
	uint32_t longitude = (uint32_t) in["longitude"].asUInt();

	m_landPointOk = true;
	m_LandPointLatitude = ((double)latitude) * 1e-7;
	m_LandPointLongitude = ((double)longitude) * 1e-7;

	sendAckMessage(linkProtocolSetLandPoint, ackMsgStateOk);
}


bool djiFlightHandler::getLandPoint(double &latitude, double &longitude)
{
	latitude = m_LandPointLatitude;
	longitude = m_LandPointLongitude;
	return m_landPointOk;
}


void djiFlightHandler::sendAckMessage(enum linkProtocolMessageId ackMsgId, enum ackMsgState status)
{
	if(false == m_mopChannelConnectedFlag)
	{
		return;
	}

	//package to json format
	Json::Value  item;
	Json::Value arrayObj;

	item["ackMsgId"] = ackMsgId;
	item["status"] = status;
	arrayObj.append(item);

	std::string out = arrayObj.toStyledString();

	if(out.length()>= LINK_PROTOCOL_DATA_BUFFER_SIZE)
	{
		writeLogFile("sendUpdateFirwareState string length is bigger than %d\n", LINK_PROTOCOL_DATA_BUFFER_SIZE);
		return;
	}

	uint8_t dataBuf[sizeof(linkProtocolType)];

	memset(dataBuf, 0, sizeof(linkProtocolType));

	int count = packageMessage(dataBuf, linkProtocolAck, out, out.length());

	writeMopChannelSendNormalBuffer(dataBuf, count);

	writeLogFile("sendAckMessage, ackmsg:%d, status:%d\n", (int)ackMsgId, (int)status);
}

bool djiFlightHandler::getCanLandFlag()
{
	return m_canLandFlag;
}

int djiFlightHandler::getNoHeartCount()
{
	if(m_isTestMode)
	{
		return 0;
	}
	return m_noHeartCount;
}

void djiFlightHandler::noHeartCountSelfAdd()
{
	m_noHeartCount++;
}

bool djiFlightHandler::startWayPoint()
{
	m_returnCode = DjiWaypointV2_Init();
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("startWayPoint DjiWaypointV2_Init code:0x%08llX \n", m_returnCode);
		return false;
	}

	m_returnCode = DjiWaypointV2_Start();
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		DjiWaypointV2_Deinit();
		writeLogFile("startWayPoint  DjiWaypointV2_Start code:0x%08llX \n", m_returnCode);
		return false;
	}

	m_returnCode = DjiWaypointV2_Deinit();
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("startWayPoint  DjiWaypointV2_Deinit code:0x%08llX \n", m_returnCode);
		return false;
	}

	writeLogFile("startWayPoint successfull.\n");
	return true;
}

bool djiFlightHandler::stopWayPoint()
{
	m_returnCode = DjiWaypointV2_Init();
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("stopWayPoint DjiWaypointV2_Init code:0x%08llX \n", m_returnCode);
		return false;
	}

	m_returnCode = DjiWaypointV2_Stop();
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		DjiWaypointV2_Deinit();
		writeLogFile("stopWayPoint  DjiWaypointV2_Stop code:0x%08llX \n", m_returnCode);
		return false;
	}

	m_returnCode = DjiWaypointV2_Deinit();
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("stopWayPoint  DjiWaypointV2_Deinit code:0x%08llX \n", m_returnCode);
		return false;
	}

	writeLogFile("stopWayPoint successfull.\n");
	return true;
}

bool djiFlightHandler::pauseWayPoint()
{
	m_returnCode = DjiWaypointV2_Init();
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("pauseWayPoint DjiWaypointV2_Init code:0x%08llX \n", m_returnCode);
		return false;
	}

	m_returnCode = DjiWaypointV2_Pause();
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		DjiWaypointV2_Deinit();
		writeLogFile("pauseWayPoint  DjiWaypointV2_Pause code:0x%08llX \n", m_returnCode);
		return false;
	}

	m_returnCode = DjiWaypointV2_Deinit();
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("pauseWayPoint  DjiWaypointV2_Deinit code:0x%08llX \n", m_returnCode);
		return false;
	}

	writeLogFile("pauseWayPoint successfull.\n");
	return true;
}

bool djiFlightHandler::resumeWayPoint()
{
	m_returnCode = DjiWaypointV2_Init();
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("resumeWayPoint DjiWaypointV2_Init code:0x%08llX \n", m_returnCode);
		return false;
	}

	m_returnCode = DjiWaypointV2_Resume();
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		DjiWaypointV2_Deinit();
		writeLogFile("resumeWayPoint  DjiWaypointV2_Resume code:0x%08llX \n", m_returnCode);
		return false;
	}

	m_returnCode = DjiWaypointV2_Deinit();
	if(m_returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
	{
		writeLogFile("resumeWayPoint  DjiWaypointV2_Deinit code:0x%08llX \n", m_returnCode);
		return false;
	}

	writeLogFile("resumeWayPoint successfull.\n");
	return true;
}


E_DjiCameraType djiFlightHandler::GetCurrentCameraType()
{
	return m_cameraType;
}



djiFlightHandler flightHandler;

